From 7b732b393db55f56d1024f2ca42fef8e685fddc1 Mon Sep 17 00:00:00 2001
From: armar-user <armar-user@kit.edu>
Date: Tue, 27 Jul 2021 13:14:29 +0200
Subject: [PATCH] updates to robotstatememory and segment.h
---
.../server/MotionMemory/MotionMemory.cpp | 2 +-
.../RobotStateMemory/RobotStateMemory.cpp | 14 +-
.../RobotStateMemory/RobotStateMemory.h | 13 +
.../RobotAPI/libraries/armem/CMakeLists.txt | 4 +-
.../RobotAPI/libraries/armem/server/Segment.h | 160 -------------
.../armem/server/{ => segment}/Segment.cpp | 0
.../libraries/armem/server/segment/Segment.h | 225 ++++++++++++++++++
.../MotionDatabase/MDBMotions/Segment.cpp | 7 +-
.../MotionDatabase/MDBMotions/Segment.h | 8 +-
.../armem_robot_state/aron/Proprioception.xml | 2 +-
.../server/description/Segment.cpp | 16 +-
.../server/description/Segment.h | 12 +-
.../server/localization/Segment.cpp | 19 +-
.../server/localization/Segment.h | 15 +-
.../server/proprioception/Segment.cpp | 14 +-
.../server/proprioception/Segment.h | 12 +-
16 files changed, 305 insertions(+), 218 deletions(-)
delete mode 100644 source/RobotAPI/libraries/armem/server/Segment.h
rename source/RobotAPI/libraries/armem/server/{ => segment}/Segment.cpp (100%)
create mode 100644 source/RobotAPI/libraries/armem/server/segment/Segment.h
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
index ef176a8ff..350810558 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
@@ -42,7 +42,7 @@ namespace armarx
}
MotionMemory::MotionMemory() :
- mdbMotions(armem::server::ComponentPluginUser::iceMemory, armem::server::ComponentPluginUser::workingMemoryMutex)
+ mdbMotions(armem::server::ComponentPluginUser::iceMemory)
{
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index c108b6e78..2867abe0a 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -102,9 +102,9 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::onInitComponent()
{
- descriptionSegment.init();
- proprioceptionSegment.init();
- localizationSegment.init();
+ descriptionSegment.onInit();
+ proprioceptionSegment.onInit();
+ localizationSegment.onInit();
commonVisu.init();
robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
@@ -141,11 +141,9 @@ namespace armarx::armem::server::robot_state
}
RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
- descriptionSegment.connect(robotUnitPrx);
-
- proprioceptionSegment.connect(robotUnitPrx);
-
- localizationSegment.connect();
+ descriptionSegment.onConnect(robotUnitPrx);
+ proprioceptionSegment.onConnect(robotUnitPrx);
+ localizationSegment.onConnect();
commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 390483349..b8db6cf0f 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -49,6 +49,19 @@ namespace armarx::plugins
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
}
+
+Reason: DictNavigator::getElement: Could not find key 'forceTorque'. But I found the following keys: [
+ EncoderGroupName, I2CUpdateRate, IterationId, JointStatusEmergencyStop,
+ JointStatusEnabled, JointStatusError, JointStatusOperation, acceleration,
+ angularVelocity.x, angularVelocity.y, angularVelocity.z, currentTarget,
+ dieTemperature, elmoTemperature, filteredvelocity, gearTemperature,
+ gravityCompensatedTorque, gravityTorque, inertiaTorque, inverseDynamicsTorque,
+ linearAcceleration.x, linearAcceleration.y, linearAcceleration.z, maxTargetCurrent,
+ motorCurrent, motorTemperature, name, orientation.qw, orientation.qx, orientation.qy,
+ orientation.qz, position, positionTarget, relativePosition, sensorBoardUpdateRate,
+ torque, torqueSensorTemperature, torqueTicks, velocity, velocityTarget, ].
+ The path was: \
+
namespace armarx::armem::server::robot_state
{
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index c606ae195..ef5f22768 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -97,8 +97,8 @@ set(LIB_FILES
server/ComponentPlugin.cpp
server/MemoryRemoteGui.cpp
server/RemoteGuiAronDataVisitor.cpp
- server/Segment.cpp
+ server/segment/Segment.cpp
server/segment/SpecializedSegment.cpp
server/query_proc/base/BaseQueryProcessorBase.cpp
@@ -221,8 +221,8 @@ set(LIB_HEADERS
server/MemoryToIceAdapter.h
server/MemoryRemoteGui.h
server/RemoteGuiAronDataVisitor.h
- server/Segment.h
+ server/segment/Segment.h
server/segment/SpecializedSegment.h
server/query_proc.h
diff --git a/source/RobotAPI/libraries/armem/server/Segment.h b/source/RobotAPI/libraries/armem/server/Segment.h
deleted file mode 100644
index 5d1c31f41..000000000
--- a/source/RobotAPI/libraries/armem/server/Segment.h
+++ /dev/null
@@ -1,160 +0,0 @@
-#pragma once
-
-// STD/STL
-#include <mutex>
-#include <string>
-
-// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-#include "MemoryToIceAdapter.h"
-
-
-namespace armarx::armem::server
-{
- /**
- * @brief A base class for memory servers to manage their segments.
- * A segment can inherit this base class and add segment specific code.
- * TODO (fabian.peller): add concept to only accept coresegments, providersegments or entitysegments
- */
- template <class SegmentType>
- class SegmentBase : public armarx::Logging
- {
- public:
- SegmentBase() = delete;
- SegmentBase(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
- iceMemory(iceMemory),
- memoryMutex(memoryMutex)
- {
- Logging::setTag("armarx::armem::Segment");
- }
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") = 0;
- virtual void onInit() = 0;
- virtual void onConnect() = 0;
-
- protected:
- private:
-
- public:
- protected:
- // Memory connection
- armem::server::MemoryToIceAdapter& iceMemory;
- std::mutex& memoryMutex;
-
- SegmentType* segment;
-
- private:
- };
-
- namespace wm
- {
- template <class BusinessClassObject>
- class CoreSegmentBase : public SegmentBase<armarx::armem::wm::CoreSegment>
- {
- using Base = SegmentBase<armarx::armem::wm::CoreSegment>;
-
- public:
- CoreSegmentBase(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex):
- Base(iceMemory, memoryMutex)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- 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).");
- }
-
- virtual void onInit() override
- {
- ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
-
- segment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, BusinessClassObject::toAronType());
- segment->setMaxHistorySize(p.maxHistorySize);
- }
-
- virtual void onConnect() override
- {
-
- }
-
- protected:
- private:
-
- public:
- protected:
- struct Properties
- {
- std::string coreSegmentName = "";
- int maxHistorySize = -1;
- };
- Properties p;
- };
-
-
- template <class BusinessClassObject>
- class ProviderSegmentBase : public SegmentBase<armarx::armem::wm::ProviderSegment>
- {
- using Base = SegmentBase<armarx::armem::wm::ProviderSegment>;
-
- public:
- ProviderSegmentBase(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex):
- Base(iceMemory, memoryMutex)
- {
- Logging::setTag("armarx::armem::wm::Segment");
- }
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
- {
- ARMARX_CHECK_NOT_NULL(defs);
-
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the core segment.");
- defs->optional(p.providerSegmentName, prefix + "ProviderSegmentName", "Name of the provider segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of 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);
- coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
- else
- {
- coreSegment = &iceMemory.workingMemory->getCoreSegment(p.coreSegmentName);
- }
-
- segment = &coreSegment->addProviderSegment(p.providerSegmentName, BusinessClassObject::toAronType());
- segment->setMaxHistorySize(p.maxHistorySize);
- }
-
- virtual void onConnect() override
- {
-
- }
-
- protected:
- private:
-
- public:
- protected:
- struct Properties
- {
- std::string coreSegmentName = "";
- std::string providerSegmentName = "";
- int maxHistorySize = -1;
- };
- Properties p;
-
- armarx::armem::wm::CoreSegment* coreSegment;
- };
- }
-}
diff --git a/source/RobotAPI/libraries/armem/server/Segment.cpp b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp
similarity index 100%
rename from source/RobotAPI/libraries/armem/server/Segment.cpp
rename to source/RobotAPI/libraries/armem/server/segment/Segment.cpp
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.h b/source/RobotAPI/libraries/armem/server/segment/Segment.h
new file mode 100644
index 000000000..6cd9761f8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.h
@@ -0,0 +1,225 @@
+#pragma once
+
+// STD/STL
+#include <mutex>
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include "../MemoryToIceAdapter.h"
+
+
+namespace armarx::armem::server::segment
+{
+ namespace detail
+ {
+ /**
+ * @brief A base class for memory servers to manage their segments.
+ * A segment can inherit this base class and add segment specific code.
+ * TODO (fabian.peller): add concept to only accept coresegments, providersegments or entitysegments
+ */
+ template <class SegmentType>
+ class SegmentBase : public armarx::Logging
+ {
+ public:
+ SegmentBase() = delete;
+ SegmentBase(armem::server::MemoryToIceAdapter& iceMemory) :
+ iceMemory(iceMemory)
+ {
+ Logging::setTag("armarx::armem::Segment");
+ }
+
+ virtual ~SegmentBase() = default;
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") = 0;
+ virtual void onInit() = 0;
+ virtual void onConnect() = 0;
+
+ protected:
+ // Memory connection
+ armem::server::MemoryToIceAdapter& iceMemory;
+
+ SegmentType* segment;
+
+ private:
+ };
+ }
+
+ namespace wm
+ {
+ /**
+ * @brief A base class for core segments
+ */
+ class CoreSegmentBase : public detail::SegmentBase<armarx::armem::wm::CoreSegment>
+ {
+ 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;
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
+ {
+ 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).");
+ }
+
+ virtual void onInit() override
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ segment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, coreSegmentAronType);
+ segment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ virtual void onConnect() override
+ {
+
+ }
+
+ std::mutex& mutex() const
+ {
+ ARMARX_CHECK_NOT_NULL(segment);
+ return segment->mutex();
+ }
+
+ protected:
+ struct Properties
+ {
+ std::string coreSegmentName;
+ int maxHistorySize;
+ };
+ Properties p;
+
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
+ };
+
+
+ /**
+ * @brief A base class for provider segments
+ */
+ class ProviderSegmentBase : public detail::SegmentBase<armarx::armem::wm::ProviderSegment>
+ {
+ 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;
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override
+ {
+ 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).");
+ }
+
+ 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 onConnect() override
+ {
+
+ }
+
+ protected:
+ struct Properties
+ {
+ std::string coreSegmentName;
+ std::string providerSegmentName;
+ int maxHistorySize;
+ };
+ Properties p;
+
+ aron::typenavigator::ObjectNavigatorPtr coreSegmentAronType;
+ aron::typenavigator::ObjectNavigatorPtr providerSegmentAronType;
+
+ armarx::armem::wm::CoreSegment* coreSegment;
+ };
+
+
+ /**
+ * @brief A base class for provider segments with an aron business object
+ */
+ template <class BusinessClassObject>
+ class AronTypedCoreSegmentBase : public CoreSegmentBase
+ {
+ using Base = CoreSegmentBase;
+
+ public:
+ AronTypedCoreSegmentBase(armem::server::MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ int defaultMaxHistorySize = -1):
+ Base(iceMemory, defaultCoreSegmentName, BusinessClassObject::toAronType(), defaultMaxHistorySize)
+ {
+ }
+
+ virtual ~AronTypedCoreSegmentBase() = default;
+ };
+
+
+ /**
+ * @brief A base class for provider segments with an aron business object
+ */
+ template <class BusinessClassObject>
+ class AronTypedProviderSegmentBase : public ProviderSegmentBase
+ {
+ 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)
+ {
+ }
+
+ virtual ~AronTypedProviderSegmentBase() = default;
+ };
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
index c9aec9401..8fab7c2c6 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.cpp
@@ -12,8 +12,8 @@
namespace armarx::armem::server::motions::mdb
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- Base(memoryToIceAdapter, memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter)
{
}
@@ -78,7 +78,8 @@ namespace armarx::armem::server::motions::mdb
{
ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
}
- }IceUtil::Time::now();
+ }
+ IceUtil::Time::now();
loadedMotions += allMotions.size();
}
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 10dd57dee..66e55f410 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionDatabase/MDBMotions/Segment.h
@@ -5,7 +5,7 @@
#include <string>
// BaseClass
-#include <RobotAPI/libraries/armem/server/Segment.h>
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
// ArmarX
#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
@@ -13,12 +13,12 @@
namespace armarx::armem::server::motions::mdb
{
- class Segment : public armem::server::wm::ProviderSegmentBase<armarx::armem::mdb::MDBReference>
+ class Segment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>
{
- using Base = armem::server::wm::ProviderSegmentBase<armarx::armem::mdb::MDBReference>;
+ using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>;
public:
- Segment(armem::server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+ Segment(armem::server::MemoryToIceAdapter& iceMemory);
virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
virtual void onInit() override;
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index 9a402c5f0..f665ea6af 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -66,7 +66,7 @@ Robot proprioception.
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
- <ObjectChild key="accelleration">
+ <ObjectChild key="acceleration">
<EigenMatrix rows="3" cols="1" type="float" />
</ObjectChild>
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 6f247e9be..bd1e9c4fc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -25,7 +25,6 @@
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -33,16 +32,17 @@ namespace armarx::armem::server::robot_state::description
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- SpecializedSegment(memoryToIceAdapter, arondto::RobotDescription::toAronType(),
- "Description")
+ Base(memoryToIceAdapter, "Description")
{
}
Segment::~Segment() = default;
- void Segment::connect(const RobotUnitInterfacePrx& robotUnitPrx)
+ void Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
{
+ Base::onConnect();
+
robotUnit = robotUnitPrx;
// store the robot description linked to the robot unit in this segment
@@ -54,8 +54,8 @@ namespace armarx::armem::server::robot_state::description
{
const Time now = TimeUtil::GetTime();
- const MemoryID providerID = coreSegment->id().withProviderSegmentName(robotDescription.name);
- coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toAronType());
+ const MemoryID providerID = segment->id().withProviderSegmentName(robotDescription.name);
+ segment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toAronType());
EntityUpdate update;
update.entityID = providerID.withEntityName("description");
@@ -113,12 +113,12 @@ namespace armarx::armem::server::robot_state::description
Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
{
- ARMARX_CHECK_NOT_NULL(coreSegment);
+ ARMARX_CHECK_NOT_NULL(segment);
(void) timestamp; // Unused
RobotDescriptionMap robotDescriptions;
{
- for (const auto& [_, provSeg] : *coreSegment)
+ for (const auto& [_, provSeg] : *segment)
{
for (const auto& [name, entity] : provSeg.entities())
{
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 d020b5543..c62821c4a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -21,31 +21,37 @@
#pragma once
+// STD / STL
#include <string>
#include <optional>
#include <mutex>
#include <unordered_map>
+// ArmarX
#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/server/segment/Segment.h>
#include <RobotAPI/libraries/armem_objects/types.h>
+// Aron
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
namespace armarx::armem::server::robot_state::description
{
- class Segment : public segment::SpecializedSegment
+ class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription>
{
+ using Base = segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription>;
+
public:
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void connect(const RobotUnitInterfacePrx& robotUnitPrx);
+ void onConnect(const RobotUnitInterfacePrx& robotUnitPrx);
/// mapping "robot name" -> "robot 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 dd0131349..ce82f0afa 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
@@ -42,8 +41,7 @@ namespace armarx::armem::server::robot_state::localization
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- SpecializedSegment(memoryToIceAdapter, arondto::Transform::toAronType(),
- "Localization", 1024)
+ Base(memoryToIceAdapter, "Localization", 1024)
{
}
@@ -51,11 +49,6 @@ namespace armarx::armem::server::robot_state::localization
Segment::~Segment() = default;
- void Segment::connect()
- {
- }
-
-
Segment::RobotFramePoseMap
Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
{
@@ -72,7 +65,7 @@ namespace armarx::armem::server::robot_state::localization
RobotFramePoseMap frames;
- const auto knownRobots = simox::alg::get_keys(*coreSegment);
+ const auto knownRobots = simox::alg::get_keys(*segment);
for (const auto& robotName : knownRobots)
{
TransformQuery query{.header{.parentFrame = GlobalFrame,
@@ -80,7 +73,7 @@ namespace armarx::armem::server::robot_state::localization
.agent = robotName,
.timestamp = timestamp}};
- const auto result = TransformHelper::lookupTransformChain(*coreSegment, query);
+ const auto result = TransformHelper::lookupTransformChain(*segment, query);
if (not result)
{
// TODO
@@ -113,7 +106,7 @@ namespace armarx::armem::server::robot_state::localization
RobotPoseMap robotGlobalPoses;
- const std::vector<std::string> knownRobots = simox::alg::get_keys(*coreSegment);
+ const std::vector<std::string> knownRobots = simox::alg::get_keys(*segment);
for (const std::string& robotName : knownRobots)
{
@@ -123,7 +116,7 @@ namespace armarx::armem::server::robot_state::localization
.agent = robotName,
.timestamp = timestamp}};
- const auto result = TransformHelper::lookupTransform(*coreSegment, query);
+ const auto result = TransformHelper::lookupTransform(*segment, query);
if (not result)
{
// TODO
@@ -163,7 +156,7 @@ namespace armarx::armem::server::robot_state::localization
EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::Transform& transform) const
{
const armem::Time& timestamp = transform.header.timestamp;
- const MemoryID providerID = coreSegment->id().withProviderSegmentName(transform.header.agent);
+ const MemoryID providerID = segment->id().withProviderSegmentName(transform.header.agent);
EntityUpdate update;
update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
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 d573c37ec..53d112362 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -21,17 +21,23 @@
#pragma once
+// STD / STL
#include <string>
#include <optional>
#include <unordered_map>
+// Eigen
#include <Eigen/Geometry>
+// ArmarX
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem/server/segment/Segment.h>
#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
+// Aron
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
namespace armarx::armem
{
@@ -40,8 +46,10 @@ namespace armarx::armem
namespace armarx::armem::server::robot_state::localization
{
- class Segment : public segment::SpecializedSegment
+ class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::Transform>
{
+ using Base = segment::wm::AronTypedCoreSegmentBase<arondto::Transform>;
+
public:
using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
@@ -53,9 +61,6 @@ namespace armarx::armem::server::robot_state::localization
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
-
- void connect();
-
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 5e01e84eb..046e7b854 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -21,7 +21,6 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
@@ -31,16 +30,17 @@ namespace armarx::armem::server::robot_state::proprioception
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- SpecializedSegment(memoryToIceAdapter, prop::arondto::Proprioception::toAronType(),
- "Proprioception", 1024)
+ Base(memoryToIceAdapter, "Proprioception", 1024)
{
}
Segment::~Segment() = default;
- void Segment::connect(RobotUnitInterfacePrx robotUnitPrx)
+ void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
+ Base::onConnect();
+
this->robotUnit = robotUnitPrx;
std::string providerSegmentName = "Robot";
@@ -54,7 +54,7 @@ namespace armarx::armem::server::robot_state::proprioception
ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
<< "\n Falling back to provider segment name '" << providerSegmentName << "'.";
}
- this->robotUnitProviderID = coreSegment->id().withProviderSegmentName(providerSegmentName);
+ this->robotUnitProviderID = segment->id().withProviderSegmentName(providerSegmentName);
}
@@ -79,12 +79,12 @@ namespace armarx::armem::server::robot_state::proprioception
DebugObserverHelper* debugObserver) const
{
namespace adn = aron::datanavigator;
- ARMARX_CHECK_NOT_NULL(coreSegment);
+ ARMARX_CHECK_NOT_NULL(segment);
RobotJointPositionMap jointMap;
TIMING_START(tProcessProviders);
- for (const auto& [robotName, provSeg] : *coreSegment)
+ for (const auto& [robotName, provSeg] : *segment)
{
TIMING_START(tProcessEntities);
int i = 0;
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 87ef6a4e2..7ee14aabc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -21,17 +21,21 @@
#pragma once
+// STD / STL
#include <string>
#include <optional>
#include <unordered_map>
+// ArmarX
#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/server/segment/Segment.h>
#include <RobotAPI/libraries/armem_objects/types.h>
+// Aron
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx
{
@@ -39,8 +43,10 @@ namespace armarx
}
namespace armarx::armem::server::robot_state::proprioception
{
- class Segment : public segment::SpecializedSegment
+ class Segment : public segment::wm::AronTypedCoreSegmentBase<prop::arondto::Proprioception>
{
+ using Base = segment::wm::AronTypedCoreSegmentBase<prop::arondto::Proprioception>;
+
public:
using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
@@ -50,7 +56,7 @@ namespace armarx::armem::server::robot_state::proprioception
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void connect(RobotUnitInterfacePrx robotUnitPrx);
+ void onConnect(RobotUnitInterfacePrx robotUnitPrx);
RobotJointPositionMap getRobotJointPositions(
--
GitLab