From 95d7a9352fd48545b5a31cc2739ded2602c54a2c Mon Sep 17 00:00:00 2001 From: Fabian Peller-Konrad <fabian.peller-konrad@kit.edu> Date: Fri, 26 Nov 2021 15:17:59 +0100 Subject: [PATCH] Updated Segment implementation (Unified two existing ones) and updated existing memories Moved Trajectory to aron::common --- .../RobotStateMemory/RobotStateMemory.cpp | 6 +- .../RobotAPI/libraries/armem/CMakeLists.txt | 8 +- .../armem/server/segment/Segment.cpp | 101 ---------- .../libraries/armem/server/segment/Segment.h | 190 ------------------ .../server/segment/SpecializedCoreSegment.cpp | 95 +++++++++ .../server/segment/SpecializedCoreSegment.h | 63 ++++++ .../segment/SpecializedProviderSegment.cpp | 101 ++++++++++ .../segment/SpecializedProviderSegment.h | 59 ++++++ .../server/segment/SpecializedSegment.cpp | 65 ------ .../armem/server/segment/SpecializedSegment.h | 63 +----- .../segment/detail/SpecializedSegment.cpp | 6 + .../segment/detail/SpecializedSegment.h | 52 +++++ .../armem_motions/aron/MDBReference.xml | 6 +- .../armem_motions/server/MotionSegment.cpp | 28 +-- .../armem_motions/server/MotionSegment.h | 15 +- .../armem_motions/server/mdb_conversions.cpp | 8 +- .../armem_motions/server/mdb_conversions.h | 2 +- .../libraries/armem_mps/CMakeLists.txt | 10 +- .../armem_mps/aron/JointSpaceTrajectory.xml | 27 --- .../armem_mps/aron/TaskSpaceTrajectory.xml | 27 --- .../armem_mps/aron/Trajectory_old.xml | 37 ---- .../aron/Trajectory_old_didworkpartially.xml | 41 ---- .../libraries/armem_mps/aron_conversions.h | 8 +- .../server/MotionPrimitives/Segment.cpp | 24 +-- .../server/MotionPrimitives/Segment.h | 15 +- .../MotionPrimitives/motionprimitives.cpp | 12 +- .../MotionPrimitives/motionprimitives.h | 7 +- .../armem_objects/server/class/Segment.cpp | 23 +-- .../armem_objects/server/class/Segment.h | 2 +- .../armem_objects/server/instance/Segment.cpp | 54 ++--- .../armem_objects/server/instance/Segment.h | 6 +- .../server/description/Segment.cpp | 10 +- .../server/description/Segment.h | 7 +- .../server/localization/Segment.cpp | 12 +- .../server/localization/Segment.h | 7 +- .../server/proprioception/Segment.cpp | 6 +- .../server/proprioception/Segment.h | 7 +- .../libraries/aron/common/CMakeLists.txt | 4 +- .../libraries/aron/common/aron/SimplePose.xml | 27 --- .../aron/common/aron/SimplePosition.xml | 26 --- .../aron/common/aron/SimpleTrajectory.xml | 26 --- .../common}/aron/Trajectory.xml | 22 +- 42 files changed, 516 insertions(+), 799 deletions(-) delete mode 100644 source/RobotAPI/libraries/armem/server/segment/Segment.cpp delete mode 100644 source/RobotAPI/libraries/armem/server/segment/Segment.h create mode 100644 source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.cpp create mode 100644 source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h create mode 100644 source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp create mode 100644 source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h create mode 100644 source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.cpp create mode 100644 source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h delete mode 100644 source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml delete mode 100644 source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml delete mode 100644 source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml delete mode 100644 source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml delete mode 100644 source/RobotAPI/libraries/aron/common/aron/SimplePose.xml delete mode 100644 source/RobotAPI/libraries/aron/common/aron/SimplePosition.xml delete mode 100644 source/RobotAPI/libraries/aron/common/aron/SimpleTrajectory.xml rename source/RobotAPI/libraries/{armem_mps => aron/common}/aron/Trajectory.xml (62%) diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp index 5128cd755..6cc5b6a88 100644 --- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp +++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp @@ -95,9 +95,9 @@ namespace armarx::armem::server::robot_state void RobotStateMemory::onInitComponent() { - descriptionSegment.onInit(); - proprioceptionSegment.onInit(); - localizationSegment.onInit(); + descriptionSegment.init(); + proprioceptionSegment.init(); + localizationSegment.init(); commonVisu.init(); robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY); diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt index 18ddb3492..d98b1b348 100644 --- a/source/RobotAPI/libraries/armem/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem/CMakeLists.txt @@ -125,7 +125,9 @@ set(LIB_FILES server/plugins/ReadOnlyPluginUser.cpp server/plugins/ReadWritePluginUser.cpp - server/segment/Segment.cpp + server/segment/detail/SpecializedSegment.cpp + server/segment/SpecializedCoreSegment.cpp + server/segment/SpecializedProviderSegment.cpp server/segment/SpecializedSegment.cpp server/query_proc/base/BaseQueryProcessorBase.cpp @@ -276,7 +278,9 @@ set(LIB_HEADERS server/plugins/ReadOnlyPluginUser.h server/plugins/ReadWritePluginUser.h - server/segment/Segment.h + server/segment/detail/SpecializedSegment.h + server/segment/SpecializedCoreSegment.h + server/segment/SpecializedProviderSegment.h server/segment/SpecializedSegment.h server/query_proc.h diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp b/source/RobotAPI/libraries/armem/server/segment/Segment.cpp deleted file mode 100644 index 4e477fd51..000000000 --- a/source/RobotAPI/libraries/armem/server/segment/Segment.cpp +++ /dev/null @@ -1,101 +0,0 @@ -#include "Segment.h" - -#include <ArmarXCore/core/application/properties/PluginAll.h> -#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h> - -#include <RobotAPI/libraries/aron/core/type/variant/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::type::ObjectPtr 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); - } - - - wm::ProviderSegmentBase::ProviderSegmentBase( - armem::server::MemoryToIceAdapter& iceMemory, - const std::string& defaultProviderSegmentName, - aron::type::ObjectPtr providerSegmentAronType, - const std::string& defaultCoreSegmentName, - aron::type::ObjectPtr 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 deleted file mode 100644 index 2ea300063..000000000 --- a/source/RobotAPI/libraries/armem/server/segment/Segment.h +++ /dev/null @@ -1,190 +0,0 @@ -#pragma once - -#include <RobotAPI/libraries/armem/server/forward_declarations.h> -#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h> - -#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h> - -#include <ArmarXCore/core/logging/Logging.h> -#include <ArmarXCore/core/application/properties/forward_declarations.h> - -#include <string> - - -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(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 - MemoryToIceAdapter& iceMemory; - - SegmentType* segment; - - }; - } - - - namespace wm - { - /** - * @brief A base class for core segments - */ - class CoreSegmentBase : public detail::SegmentBase<server::wm::CoreSegment> - { - using Base = detail::SegmentBase<server::wm::CoreSegment>; - - public: - - CoreSegmentBase( - MemoryToIceAdapter& iceMemory, - const std::string& defaultCoreSegmentName = "", - aron::type::ObjectPtr coreSegmentAronType = nullptr, - int defaultMaxHistorySize = -1); - - virtual ~CoreSegmentBase() override; - - - virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override; - virtual void onInit() override; - // virtual void onConnect() override; - - - template <class FunctionT> - auto doLocked(FunctionT&& function) const - { - return segment->doLocked(function); - } - - - protected: - - struct Properties - { - std::string coreSegmentName; - int maxHistorySize; - }; - Properties p; - - aron::type::ObjectPtr coreSegmentAronType; - - }; - - - /** - * @brief A base class for provider segments - */ - class ProviderSegmentBase : public detail::SegmentBase<server::wm::ProviderSegment> - { - using Base = detail::SegmentBase<server::wm::ProviderSegment>; - - public: - - ProviderSegmentBase( - MemoryToIceAdapter& iceMemory, - const std::string& defaultProviderSegmentName = "", - aron::type::ObjectPtr providerSegmentAronType = nullptr, - const std::string& defaultCoreSegmentName = "", - aron::type::ObjectPtr coreSegmentAronType = nullptr, - int defaultMaxHistorySize = -1); - - virtual ~ProviderSegmentBase() override; - - - virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override; - virtual void onInit() override; - // virtual void onConnect() override; - - - protected: - - struct Properties - { - std::string coreSegmentName; - std::string providerSegmentName; - int maxHistorySize; - }; - Properties p; - - aron::type::ObjectPtr coreSegmentAronType; - aron::type::ObjectPtr providerSegmentAronType; - - server::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( - 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( - 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/server/segment/SpecializedCoreSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.cpp new file mode 100644 index 000000000..32f0f8f56 --- /dev/null +++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.cpp @@ -0,0 +1,95 @@ +#include "SpecializedCoreSegment.h" + +#include <ArmarXCore/core/application/properties/PluginAll.h> +#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h> + +#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h> +#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h> + + +namespace armarx::armem::server::segment +{ + + SpecializedCoreSegment::SpecializedCoreSegment( + armem::server::MemoryToIceAdapter& iceMemory, + const std::string& defaultCoreSegmentName, + aron::type::ObjectPtr coreSegmentAronType, + int defaultMaxHistorySize) : + Base(iceMemory), + aronType(coreSegmentAronType), + properties({defaultCoreSegmentName, defaultMaxHistorySize}) + { + Logging::setTag("armarx::armem::SpecializedCoreSegment"); + } + + + SpecializedCoreSegment::~SpecializedCoreSegment() + { + } + + + void SpecializedCoreSegment::defineProperties( + armarx::PropertyDefinitionsPtr defs, + const std::string& prefix) + { + ARMARX_CHECK_NOT_NULL(defs); + + defs->optional(properties.segmentName, + prefix + "seg.CoreSegmentName", + "Name of the " + properties.segmentName + " core segment."); + + defs->optional(properties.maxHistorySize, + prefix + "seg.CoreMaxHistorySize", + "Maximal size of the " + properties.segmentName + " entity histories (-1 for infinite)."); + } + + + void SpecializedCoreSegment::init() + { + ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory); + Logging::setTag(properties.segmentName + " Core Segment"); + + if (iceMemory.workingMemory->hasCoreSegment(properties.segmentName)) + { + ARMARX_WARNING << "Core segment with name '" << properties.segmentName << "' already exists."; + segmentPtr = &iceMemory.workingMemory->getCoreSegment(properties.segmentName); + } + else + { + ARMARX_INFO << "Adding core segment '" << properties.segmentName << "'"; + segmentPtr = &iceMemory.workingMemory->addCoreSegment(properties.segmentName); + } + segmentPtr->aronType() = aronType; + segmentPtr->setMaxHistorySize(properties.maxHistorySize); + } + + void SpecializedCoreSegment::setDefaultCoreSegmentName(const std::string& coreSegmentName) + { + this->properties.segmentName = coreSegmentName; + } + + + void SpecializedCoreSegment::setDefaultMaxHistorySize(int64_t maxHistorySize) + { + this->properties.maxHistorySize = maxHistorySize; + } + + + void SpecializedCoreSegment::setAronType(aron::type::ObjectPtr aronType) + { + this->aronType = aronType; + } + + wm::CoreSegment& SpecializedCoreSegment::getCoreSegment() + { + ARMARX_CHECK_NOT_NULL(segmentPtr); + return *segmentPtr; + } + + + const wm::CoreSegment& SpecializedCoreSegment::getCoreSegment() const + { + ARMARX_CHECK_NOT_NULL(segmentPtr); + return *segmentPtr; + } +} diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h new file mode 100644 index 000000000..d94fa1aed --- /dev/null +++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h @@ -0,0 +1,63 @@ +#pragma once + +#include <RobotAPI/libraries/armem/server/forward_declarations.h> +#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h> + +#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h> + +#include <ArmarXCore/core/logging/Logging.h> +#include <ArmarXCore/core/application/properties/forward_declarations.h> + +#include <string> + +#include "detail/SpecializedSegment.h" + +namespace armarx::armem::server::segment +{ + /** + * @brief A base class for core segments + */ + class SpecializedCoreSegment : public detail::SegmentBase<server::wm::CoreSegment> + { + using Base = detail::SegmentBase<server::wm::CoreSegment>; + + public: + + SpecializedCoreSegment( + MemoryToIceAdapter& iceMemory, + const std::string& defaultCoreSegmentName = "", + aron::type::ObjectPtr coreSegmentAronType = nullptr, + int defaultMaxHistorySize = -1); + + virtual ~SpecializedCoreSegment() override; + + virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override; + virtual void init() override; + + template <class FunctionT> + auto doLocked(FunctionT&& function) const + { + return segmentPtr->doLocked(function); + } + + void setDefaultCoreSegmentName(const std::string& coreSegmentName); + void setDefaultMaxHistorySize(int64_t maxHistorySize); + void setAronType(aron::type::ObjectPtr aronType); + + wm::CoreSegment& getCoreSegment(); + const wm::CoreSegment& getCoreSegment() const; + + + public: + + aron::type::ObjectPtr aronType; + + struct Properties + { + std::string segmentName; + int64_t maxHistorySize; + }; + Properties properties; + + }; +} diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp new file mode 100644 index 000000000..6bbdc36de --- /dev/null +++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp @@ -0,0 +1,101 @@ +#include "SpecializedProviderSegment.h" + +#include <ArmarXCore/core/application/properties/PluginAll.h> +#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h> + +#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h> +#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h> + + +namespace armarx::armem::server::segment +{ + SpecializedProviderSegment::SpecializedProviderSegment( + armem::server::MemoryToIceAdapter& iceMemory, + const std::string& defaultProviderSegmentName, + const std::string& defaultCoreSegmentName, + aron::type::ObjectPtr providerSegmentAronType, + aron::type::ObjectPtr coreSegmentAronType, + int defaultMaxHistorySize) : + Base(iceMemory), + aronType(providerSegmentAronType), + coreSegment(iceMemory, defaultCoreSegmentName, coreSegmentAronType), + properties({defaultProviderSegmentName, defaultMaxHistorySize}) + { + Logging::setTag("armarx::armem::SpecializedProviderSegment"); + } + + + SpecializedProviderSegment::~SpecializedProviderSegment() + { + } + + + void SpecializedProviderSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) + { + ARMARX_CHECK_NOT_NULL(defs); + + coreSegment.defineProperties(defs, prefix); + + defs->optional(properties.segmentName, + prefix + "seg.ProviderSegmentName", + "Name of the " + properties.segmentName + " provider segment."); + + defs->optional(properties.maxHistorySize, + prefix + "seg.ProviderMaxHistorySize", + "Maximal size of the " + properties.segmentName + " entity histories (-1 for infinite)."); + } + + + void SpecializedProviderSegment::init() + { + ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory); + + coreSegment.init(); + + Logging::setTag(properties.segmentName + " Provider Segment"); + + if (coreSegment.segmentPtr->hasProviderSegment(properties.segmentName)) + { + ARMARX_WARNING << "Provider segment with name '" << properties.segmentName << "' already exists."; + segmentPtr = &coreSegment.segmentPtr->getProviderSegment(properties.segmentName); + } + else + { + ARMARX_INFO << "Adding provider segment '" << properties.segmentName << "'"; + segmentPtr = &coreSegment.segmentPtr->addProviderSegment(properties.segmentName); + } + segmentPtr->aronType() = aronType; + segmentPtr->setMaxHistorySize(properties.maxHistorySize); + } + + void SpecializedProviderSegment::setDefaultProviderSegmentName(const std::string& providerSegmentName) + { + this->properties.segmentName = providerSegmentName; + } + + + void SpecializedProviderSegment::setDefaultMaxHistorySize(int64_t maxHistorySize) + { + this->properties.maxHistorySize = maxHistorySize; + } + + + void SpecializedProviderSegment::setAronType(aron::type::ObjectPtr aronType) + { + this->aronType = aronType; + } + + wm::ProviderSegment& SpecializedProviderSegment::getProviderSegment() + { + ARMARX_CHECK_NOT_NULL(segmentPtr); + return *segmentPtr; + } + + + const wm::ProviderSegment& SpecializedProviderSegment::getProviderSegment() const + { + ARMARX_CHECK_NOT_NULL(segmentPtr); + return *segmentPtr; + } + +} diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h new file mode 100644 index 000000000..44db000d1 --- /dev/null +++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h @@ -0,0 +1,59 @@ +#pragma once + +#include <RobotAPI/libraries/armem/server/forward_declarations.h> +#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h> + +#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h> + +#include <ArmarXCore/core/logging/Logging.h> +#include <ArmarXCore/core/application/properties/forward_declarations.h> + +#include <string> + +#include "SpecializedCoreSegment.h" + +namespace armarx::armem::server::segment +{ + /** + * @brief A base class for provider segments + */ + class SpecializedProviderSegment : public detail::SegmentBase<server::wm::ProviderSegment> + { + using Base = detail::SegmentBase<server::wm::ProviderSegment>; + + public: + + SpecializedProviderSegment( + MemoryToIceAdapter& iceMemory, + const std::string& defaultProviderSegmentName = "", + const std::string& defaultCoreSegmentName = "", + aron::type::ObjectPtr providerSegmentAronType = nullptr, + aron::type::ObjectPtr coreSegmentAronType = nullptr, + int defaultMaxHistorySize = -1); + + virtual ~SpecializedProviderSegment() override; + + virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override; + virtual void init() override; + + void setDefaultProviderSegmentName(const std::string& providerSegmentName); + void setDefaultMaxHistorySize(int64_t maxHistorySize); + void setAronType(aron::type::ObjectPtr aronType); + + wm::ProviderSegment& getProviderSegment(); + const wm::ProviderSegment& getProviderSegment() const; + + + public: + + aron::type::ObjectPtr aronType; + SpecializedCoreSegment coreSegment; + + struct Properties + { + std::string segmentName; + int64_t maxHistorySize; + }; + Properties properties; + }; +} diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp index 47e8343c2..415539a39 100644 --- a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp +++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.cpp @@ -1,72 +1,7 @@ #include "SpecializedSegment.h" -#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h> -#include <ArmarXCore/core/application/properties/PluginAll.h> -#include <ArmarXCore/core/exceptions/local/ExpressionException.h> - -#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h> -#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h> - - namespace armarx::armem::server::segment { - SpecializedSegment::SpecializedSegment( - server::MemoryToIceAdapter& iceMemory, - aron::type::ObjectPtr aronType, - const std::string& defaultCoreSegmentName, - int64_t defaultMaxHistorySize - ) : - iceMemory(iceMemory), aronType(aronType) - { - setDefaultCoreSegmentName(defaultCoreSegmentName); - setDefaultMaxHistorySize(defaultMaxHistorySize); - } - - - SpecializedSegment::~SpecializedSegment() - { - } - - - void SpecializedSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) - { - defs->optional(properties.coreSegmentName, - prefix + "seg.CoreSegmentName", - "Name of the " + properties.coreSegmentName + " core segment."); - - defs->optional(properties.maxHistorySize, - prefix + "seg.MaxHistorySize", - "Maximal size of the " + properties.coreSegmentName + " entity histories (-1 for infinite)."); - } - - - void SpecializedSegment::init() - { - ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory); - Logging::setTag(properties.coreSegmentName + " Core Segment"); - - ARMARX_INFO << "Adding core segment '" << properties.coreSegmentName << "'"; - coreSegment = &iceMemory.workingMemory->addCoreSegment(properties.coreSegmentName, aronType); - coreSegment->setMaxHistorySize(properties.maxHistorySize); - } - - - void SpecializedSegment::setDefaultCoreSegmentName(const std::string& coreSegmentName) - { - this->properties.coreSegmentName = coreSegmentName; - } - - - void SpecializedSegment::setDefaultMaxHistorySize(int64_t maxHistorySize) - { - this->properties.maxHistorySize = maxHistorySize; - } - - - void SpecializedSegment::setAronType(aron::type::ObjectPtr aronType) - { - this->aronType = aronType; - } } diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h index f03598e94..d407a2379 100644 --- a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h +++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h @@ -1,68 +1,9 @@ #pragma once -#include <RobotAPI/libraries/armem/server/forward_declarations.h> -#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h> -#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h> - -#include <ArmarXCore/core/application/properties/forward_declarations.h> -#include <ArmarXCore/core/logging/Logging.h> - -#include <string> - +#include "SpecializedCoreSegment.h" +#include "SpecializedProviderSegment.h" namespace armarx::armem::server::segment { - /** - * @brief Specialized management of a core segment. - */ - class SpecializedSegment : - public armarx::Logging - { - public: - - SpecializedSegment( - server::MemoryToIceAdapter& iceMemory, - aron::type::ObjectPtr aronType = nullptr, - const std::string& defaultCoreSegmentName = "", - int64_t defaultMaxHistorySize = -1); - - virtual ~SpecializedSegment(); - - - virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = ""); - virtual void init(); - // void connect(); - - - template <class FunctionT> - auto doLocked(FunctionT&& function) const - { - return coreSegment->doLocked(function); - } - - - protected: - - void setDefaultCoreSegmentName(const std::string& coreSegmentName); - void setDefaultMaxHistorySize(int64_t maxHistorySize); - void setAronType(aron::type::ObjectPtr aronType); - - - protected: - - server::MemoryToIceAdapter& iceMemory; - server::wm::CoreSegment* coreSegment = nullptr; - aron::type::ObjectPtr aronType; - - struct Properties - { - std::string coreSegmentName = ""; - int64_t maxHistorySize = -1; - }; - Properties properties; - - - }; - } diff --git a/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.cpp new file mode 100644 index 000000000..d55f3427d --- /dev/null +++ b/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.cpp @@ -0,0 +1,6 @@ +#include "SpecializedSegment.h" + +namespace armarx::armem::server::segment +{ + +} diff --git a/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h new file mode 100644 index 000000000..f1ca29d3e --- /dev/null +++ b/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h @@ -0,0 +1,52 @@ +#pragma once + +#include <RobotAPI/libraries/armem/server/forward_declarations.h> +#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h> + +#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h> + +#include <ArmarXCore/core/logging/Logging.h> +#include <ArmarXCore/core/application/properties/forward_declarations.h> + +#include <string> + + +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(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 init() = 0; + + + public: + + SegmentType* segmentPtr; + + + protected: + + // Memory connection + MemoryToIceAdapter& iceMemory; + }; + } +} diff --git a/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml b/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml index 20f01a891..dc4428a4d 100644 --- a/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml +++ b/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml @@ -11,7 +11,7 @@ </AronIncludes> <GenerateTypes> - <Object name='armarx::armem::mdb::FileReference'> + <Object name='armarx::motion::mdb::arondto::FileReference'> <ObjectChild key='createdDate'> <time /> @@ -42,7 +42,7 @@ </ObjectChild> </Object> - <Object name='armarx::armem::mdb::MDBReference'> + <Object name='armarx::motion::mdb::arondto::MDBReference'> <ObjectChild key='id'> <int /> @@ -107,7 +107,7 @@ <ObjectChild key='attachedFiles'> <Dict> <List> - <armarx::armem::mdb::FileReference /> + <armarx::motion::mdb::arondto::FileReference /> </List> </Dict> </ObjectChild> diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp index d4dd62d61..0122739bc 100644 --- a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp +++ b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp @@ -17,42 +17,42 @@ #include <sstream> -namespace armarx::armem::server::motions::mdb +namespace armarx::armem::server::motions::mdb::segment { - MotionSegment::MotionSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : - Base(memoryToIceAdapter) + MDBMotionSegment::MDBMotionSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : + Base(memoryToIceAdapter, "MotionDatabase", "Motion", armarx::motion::mdb::arondto::MDBReference::toAronType()) { } - void MotionSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) + void MDBMotionSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) { Base::defineProperties(defs, prefix); - defs->optional(p.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from."); - defs->optional(p.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup."); + defs->optional(properties.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from."); + defs->optional(properties.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup."); } - void MotionSegment::onInit() + void MDBMotionSegment::init() { - Base::onInit(); + Base::init(); - if (p.loadFromMotionsPackage) + if (properties.loadFromMotionsPackage) { - loadByMotionFinder(p.motionsPackage); + loadByMotionFinder(properties.motionsPackage); } } - void MotionSegment::onConnect() + void MDBMotionSegment::onConnect() { } - int MotionSegment::loadByMotionFinder(const std::string& packageName) + int MDBMotionSegment::loadByMotionFinder(const std::string& packageName) { priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/"); int loadedMotions = 0; - std::cout << "Load Motions from package" << std::endl; + ARMARX_INFO << "Load Motions from package" << std::endl; { // Load MDB Motions auto allMotions = motionFinder.findAll("mdb"); @@ -71,7 +71,7 @@ namespace armarx::armem::server::motions::mdb } } ARMARX_INFO << ss.str(); - auto& entity = this->segment->addEntity(std::to_string(op->id)); + auto& entity = this->segmentPtr->addEntity(std::to_string(op->id)); auto& snapshot = entity.addSnapshot(op->createdDate); armem::wm::EntityInstance& instance = snapshot.addInstance(); diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h index dfde9a95b..de2fd746f 100644 --- a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h +++ b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h @@ -5,23 +5,24 @@ #include <string> // BaseClass -#include <RobotAPI/libraries/armem/server/segment/Segment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h> // ArmarX #include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h> -namespace armarx::armem::server::motions::mdb +namespace armarx::armem::server::motions::mdb::segment { - class MotionSegment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference> + class MDBMotionSegment : public armem::server::segment::SpecializedProviderSegment { - using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::mdb::MDBReference>; + using Base = armem::server::segment::SpecializedProviderSegment; public: - MotionSegment(armem::server::MemoryToIceAdapter& iceMemory); + MDBMotionSegment(armem::server::MemoryToIceAdapter& iceMemory); virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override; - virtual void onInit() override; + virtual void init() override; virtual void onConnect(); private: @@ -33,6 +34,6 @@ namespace armarx::armem::server::motions::mdb std::string motionsPackage = "PriorKnowledgeData"; bool loadFromMotionsPackage = true; }; - Properties p; + Properties properties; }; } diff --git a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp index a77031277..79f6c4a16 100644 --- a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp +++ b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp @@ -6,14 +6,14 @@ namespace armarx::armem::server::motions::mdb::conversion { - std::optional<armem::mdb::MDBReference> createFromFile(const std::filesystem::path& pathToInfoJson) + std::optional<armarx::motion::mdb::arondto::MDBReference> createFromFile(const std::filesystem::path& pathToInfoJson) { if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson)) { std::ifstream ifs(pathToInfoJson); std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>())); - armarx::armem::mdb::MDBReference motionReference; + armarx::motion::mdb::arondto::MDBReference motionReference; auto json = nlohmann::json::parse(file_content); motionReference.id = json["id"]; @@ -32,7 +32,7 @@ namespace armarx::armem::server::motions::mdb::conversion for (const auto& attachedFile : value) { - armarx::armem::mdb::FileReference fileRef; + armarx::motion::mdb::arondto::FileReference fileRef; fileRef.attachedToId = attachedFile["attachedToId"]; fileRef.createdDate = IceUtil::Time::microSeconds(attachedFile["createdDate"]); fileRef.createdUser = attachedFile["createdUser"]; @@ -42,8 +42,6 @@ namespace armarx::armem::server::motions::mdb::conversion motionReference.attachedFiles[key].emplace_back(fileRef); } } - - return motionReference; } else diff --git a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h index 5dae00e33..146f0c660 100644 --- a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h +++ b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h @@ -9,5 +9,5 @@ namespace armarx::armem::server::motions::mdb::conversion { - std::optional<armem::mdb::MDBReference> createFromFile(const std::filesystem::path& pathToInfoJson); + std::optional<armarx::motion::mdb::arondto::MDBReference> createFromFile(const std::filesystem::path& pathToInfoJson); } diff --git a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt index 68cc66e6b..a7763da42 100644 --- a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt @@ -13,9 +13,9 @@ armarx_add_library( ArmarXCoreObservers ${DMP_LIBRARIES} RobotAPI::Core + RobotAPI::aron::common RobotAPI::armem RobotAPI::PriorKnowledge::Motions - RobotAPI::armem_motions VirtualRobot SOURCES ./aron_conversions.cpp @@ -30,12 +30,4 @@ armarx_add_library( ) -armarx_enable_aron_file_generation_for_target( - TARGET_NAME - "${LIB_NAME}" - ARON_FILES - aron/Trajectory.xml -) - - add_library(RobotAPI::armem_mps ALIAS armem_mps) diff --git a/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml deleted file mode 100644 index 9ae048e3a..000000000 --- a/source/RobotAPI/libraries/armem_mps/aron/JointSpaceTrajectory.xml +++ /dev/null @@ -1,27 +0,0 @@ -<!-- -My nice data, representing nice information. ---> -<?xml version="1.0" encoding="UTF-8" ?> -<AronTypeDefinition> - <CodeIncludes> - </CodeIncludes> - <AronIncludes> - </AronIncludes> - <GenerateTypes> - - - - <Object name="armarx::armem_mps::arondto::Trajectory"> - - <ObjectChild key="taskSpace"> - <String /> - </ObjectChild> - - <ObjectChild key="jointSpace"> - <String /> - </ObjectChild> - - </Object> - - </GenerateTypes> -</AronTypeDefinition> diff --git a/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml b/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml deleted file mode 100644 index 9ae048e3a..000000000 --- a/source/RobotAPI/libraries/armem_mps/aron/TaskSpaceTrajectory.xml +++ /dev/null @@ -1,27 +0,0 @@ -<!-- -My nice data, representing nice information. ---> -<?xml version="1.0" encoding="UTF-8" ?> -<AronTypeDefinition> - <CodeIncludes> - </CodeIncludes> - <AronIncludes> - </AronIncludes> - <GenerateTypes> - - - - <Object name="armarx::armem_mps::arondto::Trajectory"> - - <ObjectChild key="taskSpace"> - <String /> - </ObjectChild> - - <ObjectChild key="jointSpace"> - <String /> - </ObjectChild> - - </Object> - - </GenerateTypes> -</AronTypeDefinition> diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml deleted file mode 100644 index 21f8cafdf..000000000 --- a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old.xml +++ /dev/null @@ -1,37 +0,0 @@ -<!-- -My nice data, representing nice information. ---> -<?xml version="1.0" encoding="UTF-8" ?> -<AronTypeDefinition> - <CodeIncludes> - </CodeIncludes> - <AronIncludes> - </AronIncludes> - <GenerateTypes> - - <Object name="armarx::armem_mps::arondto::Trajectory"> - - <ObjectChild key="taskSpace"> - <String /> - </ObjectChild> - - <ObjectChild key="jointSpace"> - <String /> - </ObjectChild> - - </Object> - - <Object name="armarx::armem_mps::arondto::TaskspaceTrajectory"> - <ObjectChild key="test"> - <String /> - </ObjectChild> - </Object> - - <Object name="armarx::armem_mps::arondto::JointspaceTrajectory"> - <ObjectChild key="test"> - <Float /> - </ObjectChild> - </Object> - - </GenerateTypes> -</AronTypeDefinition> diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml b/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml deleted file mode 100644 index 7bb03ba62..000000000 --- a/source/RobotAPI/libraries/armem_mps/aron/Trajectory_old_didworkpartially.xml +++ /dev/null @@ -1,41 +0,0 @@ -<!-- -My nice data, representing nice information. ---> -<?xml version="1.0" encoding="UTF-8" ?> -<AronTypeDefinition> - <CodeIncludes> - </CodeIncludes> - <AronIncludes> - </AronIncludes> - <GenerateTypes> - - <Object name="armarx::armem::arondto::TSTrajectory"> - <ObjectChild key="trajElements"> - <Dict> - <Pose /> <!-- Mapping timesteps to poses --> - </Dict> - </ObjectChild> - </Object> - - <Object name="armarx::armem::arondto::JSTrajectory"> - <ObjectChild key="trajElements"> - <Dict> - <List> - <Float /> <!-- Mapping timesteps to map of joint values --> - </List> - </Dict> - </ObjectChild> - </Object> - - - <Object name="armarx::armem::arondto::Trajectory"> - <ObjectChild key="taskSpace"> - <armarx::armem::arondto::TSTrajectory /> - </ObjectChild> - <ObjectChild key="jointSpace"> - <armarx::armem::arondto::JSTrajectory /> - </ObjectChild> - </Object> - - </GenerateTypes> -</AronTypeDefinition> diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h index 11d0a12cb..8de8754f9 100644 --- a/source/RobotAPI/libraries/armem_mps/aron_conversions.h +++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.h @@ -3,9 +3,7 @@ #include <ArmarXCore/interface/core/Profiler.h> #include <ArmarXCore/observers/ObserverObjectFactories.h> -//#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h> -#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h> -#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h> +#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h> #include <dmp/representation/trajectory.h> //#include <dmp @@ -13,7 +11,7 @@ namespace armarx { - void fromAron(const armem::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace); - void toAron(armem::arondto::Trajectory& dto, const DMP::SampledTrajectoryV2& bo_taskspace, const DMP::SampledTrajectoryV2& bo_jointspace, const std::string name); + void fromAron(const armarx::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace); + void toAron(armarx::arondto::Trajectory& dto, const DMP::SampledTrajectoryV2& bo_taskspace, const DMP::SampledTrajectoryV2& bo_jointspace, const std::string name); } diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp index 9e1a8b238..579e921ca 100644 --- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp +++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp @@ -17,7 +17,7 @@ #include <sstream> -namespace armarx::armem::mps +namespace armarx::armem::server::motions::mps::segment { MPSegment::MPSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : Base(memoryToIceAdapter, "Trajectory", "MovementPrimitive") @@ -28,17 +28,17 @@ namespace armarx::armem::mps { Base::defineProperties(defs, prefix); - defs->optional(p.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from."); - defs->optional(p.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup."); + defs->optional(properties.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from."); + defs->optional(properties.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup."); } - void MPSegment::onInit() + void MPSegment::init() { - Base::onInit(); + Base::init(); - if (p.loadFromMotionsPackage) + if (properties.loadFromMotionsPackage) { - loadByMotionFinder(p.motionsPackage); + loadByMotionFinder(properties.motionsPackage); } } @@ -80,7 +80,7 @@ namespace armarx::armem::mps void MPSegment::loadSingleMotionFinder(const std::string &pathToInfoJson, const std::string &entityName, bool taskspace) { - if (auto op = mps::createFromFile(pathToInfoJson, taskspace); op.has_value()) + if (auto op = util::createFromFile(pathToInfoJson, taskspace); op.has_value()) { std::stringstream ss; ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: "; @@ -96,16 +96,16 @@ namespace armarx::armem::mps for(const auto & entry: std::filesystem::directory_iterator(path.parent_path())){ std::string newname = "joint-trajectory" + std::string(path.filename()).erase(0, 20); if(std::string(entry.path().filename()).rfind(newname, 0) == 0){ - if (auto op2 = mps::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson) + if (auto op2 = util::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson) { op->jointSpace = op2->jointSpace; instance.data() = op->toAron(); - if(this->segment->hasEntity(entityName)){ - auto& entity = this->segment->getEntity(entityName); + if(this->segmentPtr->hasEntity(entityName)){ + auto& entity = this->segmentPtr->getEntity(entityName); auto& snapshot = entity.addSnapshot(IceUtil::Time::now()); snapshot.addInstance(instance); }else{ - auto& entity = this->segment->addEntity(entityName); + auto& entity = this->segmentPtr->addEntity(entityName); auto& snapshot = entity.addSnapshot(IceUtil::Time::now()); snapshot.addInstance(instance); } diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h index 8f8dda176..ab14043ea 100644 --- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h +++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h @@ -5,23 +5,22 @@ #include <string> // BaseClass -#include <RobotAPI/libraries/armem/server/segment/Segment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h> // ArmarX -#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h> -#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h> +#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h> -namespace armarx::armem::mps +namespace armarx::armem::server::motions::mps::segment { - class MPSegment : public armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory> + class MPSegment : public armem::server::segment::SpecializedProviderSegment { - using Base = armem::server::segment::wm::AronTypedProviderSegmentBase<armarx::armem::arondto::Trajectory>; + using Base = armem::server::segment::SpecializedProviderSegment; public: MPSegment(armem::server::MemoryToIceAdapter& iceMemory); virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override; - virtual void onInit() override; + virtual void init() override; virtual void onConnect(); private: @@ -34,6 +33,6 @@ namespace armarx::armem::mps std::string motionsPackage = "PriorKnowledgeData"; bool loadFromMotionsPackage = true; }; - Properties p; + Properties properties; }; } diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp index 598cb580a..33ba4be58 100644 --- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp +++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp @@ -7,17 +7,17 @@ #include <RobotAPI/libraries/armem/core/error.h> #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h> -#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h> +#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h> #include <dmp/representation/trajectory.h> #include <ArmarXCore/core/system/ArmarXDataPath.h> #include <ArmarXCore/core/logging/Logging.h> #include <VirtualRobot/MathTools.h> -namespace armarx::armem::mps +namespace armarx::armem::server::motions::mps::segment::util { -std::optional<arondto::Trajectory> createFromFile(const std::filesystem::__cxx11::path &pathToInfoJson, bool taskspace) +std::optional<armarx::arondto::Trajectory> createFromFile(const std::filesystem::__cxx11::path &pathToInfoJson, bool taskspace) { if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson)) @@ -30,7 +30,7 @@ std::optional<arondto::Trajectory> createFromFile(const std::filesystem::__cxx11 //traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1); std::map<double, DMP::DVec> currentTraj = traj.getPositionData();//todo trajs.push_back(traj); - arondto::Trajectory trajectory; + armarx::arondto::Trajectory trajectory; std::string name = pathToInfoJson.filename(); std::string toErase = "taskspace-trajectory-"; size_t pos = name.find(toErase); @@ -48,7 +48,7 @@ std::optional<arondto::Trajectory> createFromFile(const std::filesystem::__cxx11 Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2)); Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3)); poseMatrix.block<3, 1>(0, 3) = vec; - arondto::TSElement tselement; + armarx::arondto::TSElement tselement; tselement.timestep = element.first; tselement.pose = poseMatrix; trajectory.taskSpace.steps.push_back(tselement); @@ -62,7 +62,7 @@ std::optional<arondto::Trajectory> createFromFile(const std::filesystem::__cxx11 for(double el: element.second){ configvec.push_back(float(el)); } - arondto::JSElement jselement; + armarx::arondto::JSElement jselement; jselement.timestep = element.first; jselement.jointValues = configvec; trajectory.jointSpace.steps.push_back(jselement); diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h index be5b41fab..186074ce0 100644 --- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h +++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h @@ -7,13 +7,12 @@ #include <optional> // ArmarX -#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h> -#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h> +#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h> -namespace armarx::armem::mps +namespace armarx::armem::server::motions::mps::segment::util { - std::optional<arondto::Trajectory> createFromFile(const std::filesystem::path& pathToInfoJson, bool taskspace); + std::optional<armarx::arondto::Trajectory> createFromFile(const std::filesystem::path& pathToInfoJson, bool taskspace); } #endif // MOTIONPRIMITIVES_H diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp index 8b1ae942a..3405a0290 100644 --- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp @@ -22,8 +22,7 @@ namespace armarx::armem::server::obj::clazz { Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : - SpecializedSegment(memoryToIceAdapter, - arondto::ObjectClass::toAronType(), "Class", -1) + SpecializedCoreSegment(memoryToIceAdapter, "Class", arondto::ObjectClass::toAronType(), -1) { } @@ -35,7 +34,7 @@ namespace armarx::armem::server::obj::clazz void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) { - SpecializedSegment::defineProperties(defs, prefix); + SpecializedCoreSegment::defineProperties(defs, prefix); defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from."); defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage", @@ -47,7 +46,7 @@ namespace armarx::armem::server::obj::clazz void Segment::init() { - SpecializedSegment::init(); + SpecializedCoreSegment::init(); if (p.loadFromObjectsPackage) { @@ -61,7 +60,7 @@ namespace armarx::armem::server::obj::clazz this->arviz = arviz; floorVis.setArViz(arviz); - floorVis.updateFloorObject(*coreSegment); + floorVis.updateFloorObject(*segmentPtr); } @@ -85,7 +84,7 @@ namespace armarx::armem::server::obj::clazz const bool checkPaths = false; std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths); - const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName()); + const MemoryID providerID = segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName()); ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '" << objectFinder.getPackageName() << "' ..."; Commit commit; @@ -120,13 +119,13 @@ namespace armarx::armem::server::obj::clazz viz::Layer layerAABB = arviz.layer("Class AABB"); viz::Layer layerOOBB = arviz.layer("Class OOBB"); - if (coreSegment) + if (segmentPtr) { try { - std::optional<arondto::ObjectClass> aron = coreSegment->doLocked([this, &entityID]() + std::optional<arondto::ObjectClass> aron = doLocked([this, &entityID]() { - return coreSegment->findLatestInstanceDataAs<arondto::ObjectClass>(entityID, 0); + return segmentPtr->findLatestInstanceDataAs<arondto::ObjectClass>(entityID, 0); }); if (not aron.has_value()) { @@ -274,9 +273,9 @@ namespace armarx::armem::server::obj::clazz segment.doLocked([this, &segment]() { segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue(); - if (segment.coreSegment) + if (segment.segmentPtr) { - segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize)); + segment.segmentPtr->setMaxHistorySize(long(segment.properties.maxHistorySize)); } }); } @@ -289,7 +288,7 @@ namespace armarx::armem::server::obj::clazz showComboBox = {}; showOptionsIndex.clear(); - segment.coreSegment->forEachEntity([this](const wm::Entity & entity) + segment.segmentPtr->forEachEntity([this](const wm::Entity & entity) { std::stringstream option; option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")"; diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h index 3e9717b3e..89c13ce4c 100644 --- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h +++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h @@ -21,7 +21,7 @@ namespace armarx::armem::arondto namespace armarx::armem::server::obj::clazz { - class Segment : public segment::SpecializedSegment + class Segment : public segment::SpecializedCoreSegment { public: diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp index 1383ae804..11517212d 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp @@ -45,8 +45,8 @@ namespace armarx::armem::server::obj::instance { Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : - SpecializedSegment(memoryToIceAdapter, - arondto::ObjectInstance::toAronType(), "Instance", 64) + SpecializedCoreSegment(memoryToIceAdapter, "Instance", + arondto::ObjectInstance::toAronType(), 64) { oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf> { @@ -86,7 +86,7 @@ namespace armarx::armem::server::obj::instance void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) { - SpecializedSegment::defineProperties(defs, prefix); + SpecializedCoreSegment::defineProperties(defs, prefix); defs->optional(p.discardSnapshotsWhileAttached, prefix + "DiscardSnapshotsWhileAttached", "If true, no new snapshots are stored while an object is attached to a robot node.\n" @@ -107,7 +107,7 @@ namespace armarx::armem::server::obj::instance void Segment::init() { - SpecializedSegment::init(); + SpecializedCoreSegment::init(); if (not p.sceneSnapshotToLoad.empty()) { @@ -214,8 +214,8 @@ namespace armarx::armem::server::obj::instance { Time now = TimeUtil::GetTime(); - ARMARX_CHECK_NOT_NULL(coreSegment); - const MemoryID coreSegmentID = coreSegment->id(); + ARMARX_CHECK_NOT_NULL(segmentPtr); + const MemoryID coreSegmentID = segmentPtr->id(); Commit commit; for (const objpose::ObjectPose& pose : objectPoses) @@ -250,22 +250,6 @@ namespace armarx::armem::server::obj::instance } - wm::CoreSegment& - Segment::getCoreSegment() - { - ARMARX_CHECK_NOT_NULL(coreSegment); - return *coreSegment; - } - - - const wm::CoreSegment& - Segment::getCoreSegment() const - { - ARMARX_CHECK_NOT_NULL(coreSegment); - return *coreSegment; - } - - objpose::ObjectPoseMap Segment::getObjectPoses(IceUtil::Time now) { @@ -281,8 +265,8 @@ namespace armarx::armem::server::obj::instance const std::string& providerName, IceUtil::Time now) { - ARMARX_CHECK_NOT_NULL(coreSegment); - ObjectPoseMap objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName)); + ARMARX_CHECK_NOT_NULL(segmentPtr); + ObjectPoseMap objectPoses = getLatestObjectPoses(segmentPtr->getProviderSegment(providerName)); updateObjectPoses(objectPoses, now); return filterObjectPoses(objectPoses); } @@ -291,12 +275,12 @@ namespace armarx::armem::server::obj::instance wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName) { - ARMARX_CHECK_NOT_NULL(coreSegment); + ARMARX_CHECK_NOT_NULL(segmentPtr); armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str()); if (providerName.empty()) { wm::Entity* result = nullptr; - coreSegment->forEachProviderSegment([&result, &entityID](wm::ProviderSegment & prov) + segmentPtr->forEachProviderSegment([&result, &entityID](wm::ProviderSegment & prov) { if (prov.hasEntity(entityID.entityName)) { @@ -310,9 +294,9 @@ namespace armarx::armem::server::obj::instance else { entityID.providerSegmentName = providerName; - if (coreSegment->hasProviderSegment(providerName)) + if (segmentPtr->hasProviderSegment(providerName)) { - wm::ProviderSegment& prov = coreSegment->getProviderSegment(providerName); + wm::ProviderSegment& prov = segmentPtr->getProviderSegment(providerName); return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr; } else @@ -398,8 +382,8 @@ namespace armarx::armem::server::obj::instance objpose::ObjectPoseMap Segment::getLatestObjectPoses() const { - ARMARX_CHECK_NOT_NULL(coreSegment); - return getLatestObjectPoses(*coreSegment); + ARMARX_CHECK_NOT_NULL(segmentPtr); + return getLatestObjectPoses(*segmentPtr); } @@ -715,13 +699,13 @@ namespace armarx::armem::server::obj::instance Segment::detachAllObjectsFromRobotNodes( const objpose::DetachAllObjectsFromRobotNodesInput& input) { - ARMARX_CHECK_NOT_NULL(coreSegment); + ARMARX_CHECK_NOT_NULL(segmentPtr); const armem::Time now = armem::Time::now(); objpose::DetachAllObjectsFromRobotNodesOutput output; output.numDetached = 0; - coreSegment->forEachEntity([this, now, &input, &output](wm::Entity & entity) + segmentPtr->forEachEntity([this, now, &input, &output](wm::Entity & entity) { const arondto::ObjectInstance data = this->getLatestInstanceData(entity); if (data.pose.attachmentValid) @@ -897,7 +881,7 @@ namespace armarx::armem::server::obj::instance armem::obj::SceneSnapshot Segment::getSceneSnapshot() const { armem::obj::SceneSnapshot scene; - coreSegment->forEachEntity([&scene](wm::Entity & entity) + segmentPtr->forEachEntity([&scene](wm::Entity & entity) { try { @@ -1054,9 +1038,9 @@ namespace armarx::armem::server::obj::instance if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()) { segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue(); - if (segment.coreSegment) + if (segment.segmentPtr) { - segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize)); + segment.segmentPtr->setMaxHistorySize(long(segment.properties.maxHistorySize)); } } diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h index 27ffbc746..51972e678 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h +++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h @@ -36,7 +36,7 @@ namespace armarx::armem::arondto namespace armarx::armem::server::obj::instance { - class Segment : public segment::SpecializedSegment + class Segment : public segment::SpecializedCoreSegment { public: @@ -68,10 +68,6 @@ namespace armarx::armem::server::obj::instance void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = ""); - wm::CoreSegment& getCoreSegment(); - const wm::CoreSegment& getCoreSegment() const; - - objpose::ObjectPoseMap getObjectPoses(IceUtil::Time now); objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now); 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 7ca3f3701..cada3f560 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp @@ -26,7 +26,7 @@ namespace armarx::armem::server::robot_state::description { Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : - Base(memoryToIceAdapter, "Description") + Base(memoryToIceAdapter, "Description", arondto::RobotDescription::toAronType()) { } @@ -46,8 +46,8 @@ namespace armarx::armem::server::robot_state::description { const Time now = TimeUtil::GetTime(); - const MemoryID providerID = segment->id().withProviderSegmentName(robotDescription.name); - segment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toAronType()); + const MemoryID providerID = segmentPtr->id().withProviderSegmentName(robotDescription.name); + segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toAronType()); EntityUpdate update; update.entityID = providerID.withEntityName("description"); @@ -109,11 +109,11 @@ namespace armarx::armem::server::robot_state::description RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const { - ARMARX_CHECK_NOT_NULL(segment); + ARMARX_CHECK_NOT_NULL(segmentPtr); (void) timestamp; // Unused RobotDescriptionMap robotDescriptions; - segment->forEachEntity([this, &robotDescriptions](const wm::Entity & entity) + segmentPtr->forEachEntity([this, &robotDescriptions](const wm::Entity & entity) { const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0); const auto description = robot::convertRobotDescription(entityInstance); 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 40eac5770..fb617db14 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h +++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h @@ -29,7 +29,8 @@ // ArmarX #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h> -#include <RobotAPI/libraries/armem/server/segment/Segment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h> #include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h> // Aron @@ -39,9 +40,9 @@ namespace armarx::armem::server::robot_state::description { - class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription> + class Segment : public segment::SpecializedCoreSegment { - using Base = segment::wm::AronTypedCoreSegmentBase<arondto::RobotDescription>; + using Base = segment::SpecializedCoreSegment; public: 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 1a73d571d..897858e4c 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp @@ -26,7 +26,7 @@ namespace armarx::armem::server::robot_state::localization { Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) : - Base(memoryToIceAdapter, "Localization", 1024) + Base(memoryToIceAdapter, "Localization", arondto::Transform::toAronType(), 1024) { } @@ -58,7 +58,7 @@ namespace armarx::armem::server::robot_state::localization using common::robot_state::localization::TransformQuery; RobotFramePoseMap frames; - for (const std::string& robotName : segment->getProviderSegmentNames()) + for (const std::string& robotName : segmentPtr->getProviderSegmentNames()) { TransformQuery query { @@ -70,7 +70,7 @@ namespace armarx::armem::server::robot_state::localization } }; - const auto result = TransformHelper::lookupTransformChain(*segment, query); + const auto result = TransformHelper::lookupTransformChain(*segmentPtr, query); if (not result) { // TODO @@ -104,7 +104,7 @@ namespace armarx::armem::server::robot_state::localization using common::robot_state::localization::TransformQuery; RobotPoseMap robotGlobalPoses; - for (const std::string& robotName : segment->getProviderSegmentNames()) + for (const std::string& robotName : segmentPtr->getProviderSegmentNames()) { TransformQuery query { @@ -117,7 +117,7 @@ namespace armarx::armem::server::robot_state::localization } }; - if (const auto result = TransformHelper::lookupTransform(*segment, query)) + if (const auto result = TransformHelper::lookupTransform(*segmentPtr, query)) { robotGlobalPoses.emplace(robotName, result.transform.transform); } @@ -157,7 +157,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 = segment->id().withProviderSegmentName(transform.header.agent); + const MemoryID providerID = segmentPtr->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 f0b228800..ba7138fd0 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h +++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h @@ -31,7 +31,8 @@ // ArmarX #include <RobotAPI/libraries/armem/core/forward_declarations.h> -#include <RobotAPI/libraries/armem/server/segment/Segment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h> #include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h> #include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h> @@ -39,9 +40,9 @@ namespace armarx::armem::server::robot_state::localization { - class Segment : public segment::wm::AronTypedCoreSegmentBase<arondto::Transform> + class Segment : public segment::SpecializedCoreSegment { - using Base = segment::wm::AronTypedCoreSegmentBase<arondto::Transform>; + using Base = segment::SpecializedCoreSegment; public: 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 28d1ca4e0..4f2c301e0 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp @@ -39,7 +39,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 = segment->id().withProviderSegmentName(providerSegmentName); + this->robotUnitProviderID = segmentPtr->id().withProviderSegmentName(providerSegmentName); } @@ -73,14 +73,14 @@ namespace armarx::armem::server::robot_state::proprioception DebugObserverHelper* debugObserver) const { namespace adn = aron::data; - ARMARX_CHECK_NOT_NULL(segment); + ARMARX_CHECK_NOT_NULL(segmentPtr); RobotJointPositionMap jointMap; int i = 0; Duration tFindData = Duration::milliSeconds(0), tReadJointPositions = Duration::milliSeconds(0); TIMING_START(tProcessEntities) - segment->forEachEntity([&](const wm::Entity & entity) + segmentPtr->forEachEntity([&](const wm::Entity & entity) { adn::DictPtr data; { 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 e27984bba..808fbd790 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h @@ -32,7 +32,8 @@ // RobotAPI #include <RobotAPI/libraries/armem/core/MemoryID.h> -#include <RobotAPI/libraries/armem/server/segment/Segment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h> +#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h> #include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h> @@ -42,9 +43,9 @@ namespace armarx } namespace armarx::armem::server::robot_state::proprioception { - class Segment : public segment::wm::CoreSegmentBase + class Segment : public segment::SpecializedCoreSegment { - using Base = segment::wm::CoreSegmentBase; + using Base = segment::SpecializedCoreSegment; public: diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt index b35b5cca8..ea66ba9db 100644 --- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt +++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt @@ -32,9 +32,7 @@ armarx_enable_aron_file_generation_for_target( TARGET_NAME "${LIB_NAME}" ARON_FILES - #aron/SimplePosition.xml - #aron/SimplePose.xml - #aron/SimpleTrajectory.xml + aron/Trajectory.xml aron/Color.xml aron/PackagePath.xml aron/AxisAlignedBoundingBox.xml diff --git a/source/RobotAPI/libraries/aron/common/aron/SimplePose.xml b/source/RobotAPI/libraries/aron/common/aron/SimplePose.xml deleted file mode 100644 index add970d13..000000000 --- a/source/RobotAPI/libraries/aron/common/aron/SimplePose.xml +++ /dev/null @@ -1,27 +0,0 @@ -<!--Some fancy comment -<?xml version="1.0" encoding="UTF-8" ?> -<AronTypeDefinition> - - <CodeIncludes> - <Include include="<Eigen/Core>" /> - </CodeIncludes> - - <AronIncludes> - <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/> - </AronIncludes> - - <GenerateTypes> - <Object name='armarx::armem::mmm::KeyPoint'> - - </Object> - <Object name='armarx::armem::mmm::SingleKeyPointTrajectory'> - - <ObjectChild key='name'> - < /> - </ObjectChild> - - </Object> - </GenerateTypes> - -</AronTypeDefinition> ---> diff --git a/source/RobotAPI/libraries/aron/common/aron/SimplePosition.xml b/source/RobotAPI/libraries/aron/common/aron/SimplePosition.xml deleted file mode 100644 index 42faeef90..000000000 --- a/source/RobotAPI/libraries/aron/common/aron/SimplePosition.xml +++ /dev/null @@ -1,26 +0,0 @@ -<!--Some fancy comment -<?xml version="1.0" encoding="UTF-8" ?> -<AronTypeDefinition> - - <CodeIncludes> - <Include include="<Eigen/Core>" /> - </CodeIncludes> - - <AronIncludes> - <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/> - </AronIncludes> - - <GenerateTypes> - <Object name='armarx::armem::mmm::KeyPoint'> - - </Object> - <Object name='armarx::armem::mmm::SingleKeyPointTrajectory'> - - <ObjectChild key='name'> - < /> - </ObjectChild> - - </Object> - </GenerateTypes> - -</AronTypeDefinition>--> diff --git a/source/RobotAPI/libraries/aron/common/aron/SimpleTrajectory.xml b/source/RobotAPI/libraries/aron/common/aron/SimpleTrajectory.xml deleted file mode 100644 index 42faeef90..000000000 --- a/source/RobotAPI/libraries/aron/common/aron/SimpleTrajectory.xml +++ /dev/null @@ -1,26 +0,0 @@ -<!--Some fancy comment -<?xml version="1.0" encoding="UTF-8" ?> -<AronTypeDefinition> - - <CodeIncludes> - <Include include="<Eigen/Core>" /> - </CodeIncludes> - - <AronIncludes> - <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/> - </AronIncludes> - - <GenerateTypes> - <Object name='armarx::armem::mmm::KeyPoint'> - - </Object> - <Object name='armarx::armem::mmm::SingleKeyPointTrajectory'> - - <ObjectChild key='name'> - < /> - </ObjectChild> - - </Object> - </GenerateTypes> - -</AronTypeDefinition>--> diff --git a/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml b/source/RobotAPI/libraries/aron/common/aron/Trajectory.xml similarity index 62% rename from source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml rename to source/RobotAPI/libraries/aron/common/aron/Trajectory.xml index abb2ad7c8..beb4f2788 100644 --- a/source/RobotAPI/libraries/armem_mps/aron/Trajectory.xml +++ b/source/RobotAPI/libraries/aron/common/aron/Trajectory.xml @@ -3,13 +3,9 @@ My nice data, representing nice information. --> <?xml version="1.0" encoding="UTF-8" ?> <AronTypeDefinition> - <CodeIncludes> - </CodeIncludes> - <AronIncludes> - </AronIncludes> <GenerateTypes> - <Object name="armarx::armem::arondto::TSElement"> + <Object name="armarx::arondto::TSElement"> <ObjectChild key="timestep"> <Float /> </ObjectChild> @@ -18,16 +14,16 @@ My nice data, representing nice information. </ObjectChild> </Object> - <Object name="armarx::armem::arondto::TSTrajectory"> + <Object name="armarx::arondto::TSTrajectory"> <ObjectChild key="steps"> <List> - <armarx::armem::arondto::TSElement /> <!-- Mapping timesteps to map of joint values --> + <armarx::arondto::TSElement /> <!-- Mapping timesteps to map of joint values --> </List> </ObjectChild> </Object> - <Object name="armarx::armem::arondto::JSElement"> + <Object name="armarx::arondto::JSElement"> <ObjectChild key="timestep"> <Float /> </ObjectChild> @@ -38,25 +34,25 @@ My nice data, representing nice information. </ObjectChild> </Object> - <Object name="armarx::armem::arondto::JSTrajectory"> + <Object name="armarx::arondto::JSTrajectory"> <ObjectChild key="steps"> <List> - <armarx::armem::arondto::JSElement /> <!-- Mapping timesteps to map of joint values --> + <armarx::arondto::JSElement /> <!-- Mapping timesteps to map of joint values --> </List> </ObjectChild> </Object> - <Object name="armarx::armem::arondto::Trajectory"> + <Object name="armarx::arondto::Trajectory"> <ObjectChild key="name"> <String /> </ObjectChild> <ObjectChild key="taskSpace"> - <armarx::armem::arondto::TSTrajectory /> + <armarx::arondto::TSTrajectory /> </ObjectChild> <ObjectChild key="jointSpace"> - <armarx::armem::arondto::JSTrajectory /> + <armarx::arondto::JSTrajectory /> </ObjectChild> </Object> -- GitLab