diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 86d1123051adb3bde67b885c6ba04a4d76736ea3..137453970c1a97f0f3bd9653c067e959db21b4a5 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -4,8 +4,11 @@ armarx_component_set_name("RobotStateMemory")
set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces RobotAPIComponentPlugins armem
- # RobotAPIComponentPlugins # for ArViz and other plugins
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPIComponentPlugins
+ armem
+ armem_robot_state
)
set(SOURCES
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 3c45114cacec2e232507add552b5e69a41d4236a..6d6981ff5c6076279d52b20d32bd0d956cbf7ec8 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -21,11 +21,14 @@
*/
// STD
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <algorithm>
#include <iostream>
#include <fstream>
// Header
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/core/Pose.h"
#include "RobotStateMemory.h"
// Simox
@@ -43,6 +46,12 @@
namespace armarx::armem::server::robot_state
{
RobotStateMemory::RobotStateMemory()
+ : descriptionSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ proprioceptionSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ localizationSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex)
{
}
@@ -54,6 +63,10 @@ namespace armarx::armem::server::robot_state
defs->optional(robotUnitPollFrequency, "RobotUnitUpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
defs->optional(robotUnitConfigPath, "robotUnitConfigPath", "Specify a configuration file to group the sensor values specifically.");
+ descriptionSegment.defineProperties(defs);
+ proprioceptionSegment.defineProperties(defs);
+ localizationSegment.defineProperties(defs);
+
return defs;
}
@@ -66,10 +79,14 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::onInitComponent()
{
+
+ descriptionSegment.init();
+ proprioceptionSegment.init();
+ localizationSegment.init();
+
robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
-
Ice::StringSeq includePaths;
auto packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
@@ -95,9 +112,12 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::onConnectComponent()
{
- setupRobotUnitSegment();
+ descriptionSegment.connect(getArvizClient(), getRobotUnit());
+
+ proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
+ toIce(robotUnitProviderID, proprioceptionSegment.getRobotUnitProviderID());
- setupLocalizationCoreSegment();
+ localizationSegment.connect(getArvizClient());
cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
@@ -124,45 +144,6 @@ namespace armarx::armem::server::robot_state
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
- void RobotStateMemory::setupLocalizationCoreSegment()
- {
- ARMARX_INFO << "Adding core segment " << localizationCoreSegmentName;
- workingMemory.addCoreSegments({localizationCoreSegmentName});
- }
-
- /*************************************************************/
- // RobotUnit Streaming functions
- /*************************************************************/
- void RobotStateMemory::setupRobotUnitSegment()
- {
- ARMARX_INFO << "Adding core segment " << proprioceptionCoreSegmentName;
- workingMemory.addCoreSegments({proprioceptionCoreSegmentName});
-
- ARMARX_INFO << "Adding provider segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName;
- armem::data::AddSegmentInput input;
- input.coreSegmentName = proprioceptionCoreSegmentName;
- input.providerSegmentName = robotUnitProviderSegmentName;
-
- auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
- auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
- auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
- encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
- encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
- //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
- //encoderEntryType->addMemberType("value", encoderValueType);
-
- auto result = addSegments({input})[0];
-
- if (!result.success)
- {
- ARMARX_ERROR << "Could not add segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName << ". The error message is: " << result.errorMessage;
- }
-
- robotUnitProviderID.memoryName = workingMemoryName;
- robotUnitProviderID.coreSegmentName = proprioceptionCoreSegmentName;
- robotUnitProviderID.providerSegmentName = robotUnitProviderSegmentName;
- }
-
void RobotStateMemory::convertRobotUnitStreamingDataToAron()
{
auto& data = handler->getDataBuffer();
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 015c7bd5510cd35dabccdb1c15b0b6a39bdc8081..6917665b57c26444fb383eee501b06e7fcad13e2 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -30,15 +30,20 @@
// Simox
#include <SimoxUtility/meta/enum/adapt_enum.h>
-// BaseClass
+// ArmarX
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+// BaseClass
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
+
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
-// ArmarX
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
+#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
namespace armarx::armem::server::robot_state
{
@@ -56,8 +61,8 @@ namespace armarx::armem::server::robot_state
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
- virtual public RobotUnitComponentPluginUser
-
+ virtual public RobotUnitComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser
{
public:
RobotStateMemory();
@@ -83,32 +88,29 @@ namespace armarx::armem::server::robot_state
private:
// RobotUnit Streaming
- void setupRobotUnitSegment();
-
void convertRobotUnitStreamingDataToAron();
void storeConvertedRobotUnitDataInMemory();
void startRobotUnitStream();
void stopRobotUnitStream();
- // localization segment
- void setupLocalizationCoreSegment();
-
private:
std::string workingMemoryName = "RobotStateMemory";
bool addCoreSegmentOnUsage = false;
mutable std::recursive_mutex startStopMutex;
- // Memory IDs
+ // Core segments
+
+ // - description
+ description::Segment descriptionSegment;
// - proprioception
- std::string proprioceptionCoreSegmentName = "Proprioception";
- std::string robotUnitProviderSegmentName = "RobotUnit"; // get robot name?
+ proprioception::Segment proprioceptionSegment;
armem::data::MemoryID robotUnitProviderID;
// - localization
- std::string localizationCoreSegmentName = "Localization";
+ localization::Segment localizationSegment;
// RobotUnit stuff
RobotUnitDataStreaming::DataStreamingDescription keys;
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 1778ba42d2c67c1d6967485ee5fbcaa05241488a..ace5dff8c0498522fd979a2b6a371c8dd160ecd9 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -20,7 +20,8 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(armem_gui)
add_subdirectory(armem_objects)
-add_subdirectory(armem_robot_localization)
+add_subdirectory(armem_robot)
+add_subdirectory(armem_robot_state)
add_subdirectory(armem_robot_mapping)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 03f509ce8d6cd2ddcce5c2800e14a8cafb25dfa1..dbb5033f19cd3661131ff12e092d2894ab3c2e2d 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -217,7 +217,7 @@ armarx_enable_aron_file_generation_for_target(
)
-add_library(RobotAPI::libraries::armem ALIAS "${LIB_NAME}")
+add_library(RobotAPI::armem ALIAS "${LIB_NAME}")
# add unit tests
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index b491f16ce361bb4c5ca46b1f46104b6cb302711e..ed02a9adf315ef755af2c8da64d4a37c3d4817b6 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -14,7 +14,8 @@ armarx_add_library(
RobotAPI::ArViz
RobotAPI::ComponentPlugins
RobotAPI::Core
- RobotAPI::libraries::armem
+ RobotAPI::armem
+ RobotAPI::armem_robot
HEADERS
aron_conversions.h
aron_forward_declarations.h
@@ -36,9 +37,6 @@ armarx_add_library(
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
- articulated_object_conversions.cpp
-
-
SOURCES
aron_conversions.cpp
@@ -61,8 +59,6 @@ armarx_add_library(
client/articulated_object/Writer.h
client/articulated_object/interfaces.h
- articulated_object_conversions.h
-
)
@@ -75,9 +71,6 @@ armarx_enable_aron_file_generation_for_target(
aron/ObjectInstance.xml
# aron/Attachment.xml
- aron/RobotDescription.xml
- aron/RobotState.xml
- aron/Robot.xml
aron/Constraint.xml
)
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 12066bc173c19dac13850db7fc61b07aed22ec8b..001f69f9890a27067871dc893fcdc42675618990 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -30,61 +30,4 @@ namespace armarx::armem
objpose::toAron(dto.pose, bo);
}
-
- /* to be moved */
- void fromAron(const long& dto, IceUtil::Time& time)
- {
- time = IceUtil::Time::microSeconds(dto);
- }
-
- void toAron(long& dto, const IceUtil::Time& time)
- {
- dto = time.toMicroSeconds();
- }
-
-
- /* Robot */
-
- void fromAron(const arondto::Robot& dto, Robot& bo)
- {
- // fromAron(dto.description, bo.description);
- fromAron(dto.state, bo.config);
- }
-
- void toAron(arondto::Robot& dto, const Robot& bo)
- {
- // toAron(dto.description, bo.description);
- toAron(dto.state, bo.config);
- }
-
- /* RobotDescription */
-
- void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
- {
- aron::fromAron(dto.name, bo.name);
- fromAron(dto.xml, bo.xml);
- }
-
- void toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
- {
- aron::toAron(dto.name, bo.name);
- toAron(dto.xml, bo.xml);
- }
-
- /* RobotState */
-
- void fromAron(const arondto::RobotState& dto, RobotState& bo)
- {
- fromAron(dto.timestamp, bo.timestamp);
- bo.globalPose.matrix() = dto.globalPose;
- bo.jointMap = dto.jointMap;
- }
-
- void toAron(arondto::RobotState& dto, const RobotState& bo)
- {
- toAron(dto.timestamp, bo.timestamp);
- dto.globalPose = bo.globalPose.matrix();
- dto.jointMap = bo.jointMap;
- }
-
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index 64dd1ac67e944867927ba4e51a25dbaf6c6c428d..735c9a481f60ae4bb37624bcba99a22103780cf1 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -3,14 +3,8 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-
-
#include <RobotAPI/libraries/armem_objects/types.h>
-#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/RobotState.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
-
namespace armarx::armem
{
void fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo);
@@ -19,20 +13,4 @@ namespace armarx::armem
void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
-
- // TODO move the following
- void fromAron(const long& dto, IceUtil::Time& time);
- void toAron(long& dto, const IceUtil::Time& time);
- // end TODO
-
-
- void fromAron(const arondto::Robot& dto, Robot& bo);
- void toAron(arondto::Robot& dto, const Robot& bo);
-
- void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
- void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
-
- void fromAron(const arondto::RobotState& dto, RobotState& bo);
- void toAron(arondto::RobotState& dto, const RobotState& bo);
-
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 19359fabc2dfd68c475b31241cd8f2b24b2b9fa0..03173cf876b1c77a4c68dcff4e7962a21d784164 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -8,10 +8,10 @@
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include "RobotAPI/libraries/armem_objects/articulated_object_conversions.h"
namespace fs = ::std::filesystem;
@@ -63,7 +63,7 @@ namespace armarx::armem::articulated_object
obj.config = std::move(*state);
}
- std::optional<RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -88,7 +88,7 @@ namespace armarx::armem::articulated_object
return getRobotDescription(qResult.memory);
}
- std::optional<RobotState> Reader::queryState(const RobotDescription& description, const armem::Time& timestamp)
+ std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -116,7 +116,7 @@ namespace armarx::armem::articulated_object
}
- std::optional<RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -142,12 +142,12 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return convertRobotState(instance);
+ return robot::convertRobotState(instance);
}
- std::optional<RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -173,7 +173,7 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return convertRobotDescription(instance);
+ return robot::convertRobotDescription(instance);
}
} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index cd047cf6cc81dca29061f679537d787935ebfa75..2ad108d0f300109e42be4041b199655a46d18703 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -44,15 +44,15 @@ namespace armarx::armem::articulated_object
std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
- std::optional<RobotState> queryState(const RobotDescription& description, const armem::Time& timestamp);
- std::optional<RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+ std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
+ std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
// TODO(fabian.reister): register property defs
protected:
- std::optional<RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
- std::optional<RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
+ std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
private:
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index cf057be299a48eb3e289a03886bd39c9a16046a3..53e5f925adc52aad31444071d7a7b6ded605c84f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -7,8 +7,9 @@
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
@@ -173,7 +174,7 @@ namespace armarx::armem::articulated_object
update.entityID = entityID;
arondto::Robot aronArticulatedObject;
- toAron(aronArticulatedObject, obj);
+ robot::toAron(aronArticulatedObject, obj);
const auto descriptionId = storeOrGetClass(obj);
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
index e2babe5079ff5a0e00d07317d163c089644db77b..51375ca0709d44c0c0407b7a9ac08025fa642237 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -15,9 +15,9 @@
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include "RobotAPI/libraries/armem_objects/articulated_object_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
namespace armarx::armem::server::obj::articulated_object_class
@@ -61,7 +61,7 @@ namespace armarx::armem::server::obj::articulated_object_class
for (const auto& [name, entity] : provSeg.entities())
{
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = articulated_object::convertRobotDescription(entityInstance);
+ const auto description = robot::convertRobotDescription(entityInstance);
if (not description)
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
index 9418254fff97b79fdca0c5abc55b53378c33ae8b..8932734ec4eb10ea99976786ec9e3504182f3f12 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
@@ -17,34 +17,16 @@
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/articulated_object_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+
#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
#include "Visu.h"
-namespace simox::alg
-{
- /// Get the keys of `map` in a vector.
- template <class K, class V, template<class...> class MapT = std::unordered_map, class...Ts>
- std::vector<K> get_keys2(const MapT<K, V, Ts...>& map)
- {
- std::vector<K> keys;
- if constexpr(std::is_same_v<std::unordered_map<K, V, Ts...>, MapT<K, V, Ts...>>)
- {
- keys.reserve(map.size());
- }
- for (const auto& [k, v] : map)
- {
- keys.emplace_back(k);
- }
- return keys;
- }
-}
-
namespace armarx::armem::server::obj::articulated_object_instance
{
@@ -94,7 +76,7 @@ namespace armarx::armem::server::obj::articulated_object_instance
fromAron(aronDescriptionLink, descriptionLink);
ARMARX_DEBUG << "Lookup key is " << descriptionLink;
-
+
// const auto keys = simox::alg::get_keys(knownObjectClasses);
// ARMARX_DEBUG << "Known keys " << keys;
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 5d2b4f76c83fa5ed3e823cc2d1528d941dcad6f5..cb348cd3a9731b36f2b18b7b7f366daa9b2b1f89 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -1,54 +1,13 @@
#pragma once
-// these types should be core types
-
-#include <map>
#include <vector>
-#include <filesystem>
-
-#include <Eigen/Geometry>
-#include <IceUtil/Time.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
-#include <ArmarXCore/core/PackagePath.h>
-
-namespace armarx::armem
+namespace armarx::armem::articulated_object
{
+ using ArticulatedObjectDescription = armarx::armem::robot::RobotDescription;
- struct RobotDescription
- {
- // IceUtil::Time timestamp;
-
- std::string name;
- PackagePath xml{"", std::filesystem::path("")};
-
- };
-
-
- struct RobotState
- {
- IceUtil::Time timestamp;
-
- Eigen::Affine3f globalPose;
- std::map<std::string, float> jointMap;
- };
-
- struct Robot
- {
- RobotDescription description;
- std::string instance;
-
- RobotState config;
-
- IceUtil::Time timestamp;
- };
-
- namespace articulated_object
- {
- using ArticulatedObjectDescription = RobotDescription;
-
- using ArticulatedObject = Robot;
- using ArticulatedObjects = std::vector<ArticulatedObject>;
- }
-
-} // namespace armarx::armem
\ No newline at end of file
+ using ArticulatedObject = armarx::armem::robot::Robot;
+ using ArticulatedObjects = armarx::armem::robot::Robots;
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..eb0c4fb7d37a4bb43830f224d3a81714f1175f39
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
@@ -0,0 +1,43 @@
+set(LIB_NAME armem_robot)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # ArmarXGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+ RobotAPI::Core
+ RobotAPI::armem
+ HEADERS
+ types.h
+
+ aron_conversions.h
+ robot_conversions.h
+
+ SOURCES
+
+ aron_conversions.cpp
+ robot_conversions.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/RobotDescription.xml
+ aron/RobotState.xml
+ aron/Robot.xml
+)
+
+add_library(${PROJECT_NAME}::armem_robot ALIAS armem_robot)
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
similarity index 78%
rename from source/RobotAPI/libraries/armem_objects/aron/Robot.xml
rename to source/RobotAPI/libraries/armem_robot/aron/Robot.xml
index cc005ec0521c2b6f9dcb5334a14e61337ece5cf1..63e87ab3c4ba7193a8b7800ed4c24881c09e1b45 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/Robot.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
@@ -2,13 +2,13 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>" />
- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotState.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>" />
<Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
<!-- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotDescription.xml>" /> -->
- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotState.xml>" />
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotState.xml>" />
<Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
</AronIncludes>
<GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml
rename to source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
diff --git a/source/RobotAPI/libraries/armem_objects/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_objects/aron/RobotState.xml
rename to source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a56d19f385af667974269c72c0de37af406da9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -0,0 +1,66 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+
+namespace armarx::armem::robot
+{
+
+ /* to be moved */
+ void fromAron(const long& dto, IceUtil::Time& time)
+ {
+ time = IceUtil::Time::microSeconds(dto);
+ }
+
+ void toAron(long& dto, const IceUtil::Time& time)
+ {
+ dto = time.toMicroSeconds();
+ }
+
+
+ /* Robot */
+
+ void fromAron(const arondto::Robot& dto, Robot& bo)
+ {
+ // fromAron(dto.description, bo.description);
+ fromAron(dto.state, bo.config);
+ }
+
+ void toAron(arondto::Robot& dto, const Robot& bo)
+ {
+ // toAron(dto.description, bo.description);
+ toAron(dto.state, bo.config);
+ }
+
+ /* RobotDescription */
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
+ {
+ aron::fromAron(dto.name, bo.name);
+ fromAron(dto.xml, bo.xml);
+ }
+
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
+ {
+ aron::toAron(dto.name, bo.name);
+ toAron(dto.xml, bo.xml);
+ }
+
+ /* RobotState */
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo)
+ {
+ fromAron(dto.timestamp, bo.timestamp);
+ bo.globalPose.matrix() = dto.globalPose;
+ bo.jointMap = dto.jointMap;
+ }
+
+ void toAron(arondto::RobotState& dto, const RobotState& bo)
+ {
+ toAron(dto.timestamp, bo.timestamp);
+ dto.globalPose = bo.globalPose.matrix();
+ dto.jointMap = bo.jointMap;
+ }
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..70e06ce8eedb7de9538f5249b3eedf797aec6060
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+namespace armarx::armem::robot
+{
+ // TODO move the following
+ void fromAron(const long& dto, IceUtil::Time& time);
+ void toAron(long& dto, const IceUtil::Time& time);
+ // end TODO
+
+ void fromAron(const arondto::Robot& dto, Robot& bo);
+ void toAron(arondto::Robot& dto, const Robot& bo);
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo);
+ void toAron(arondto::RobotState& dto, const RobotState& bo);
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
similarity index 85%
rename from source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp
rename to source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
index 03cac43717941e8da1a3a738d6c15aad1aa8987d..6c05e89e038c6f80f61e3e9c47f25bf8b2f1335f 100644
--- a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
@@ -1,4 +1,4 @@
-#include "articulated_object_conversions.h"
+#include "robot_conversions.h"
#include <filesystem>
@@ -7,14 +7,13 @@
#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
namespace fs = ::std::filesystem;
-namespace armarx::armem::articulated_object
+namespace armarx::armem::robot
{
std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance)
@@ -30,7 +29,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
RobotDescription robotDescription
{
.name = "",
@@ -62,4 +60,4 @@ namespace armarx::armem::articulated_object
return robotState;
}
-}
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h b/source/RobotAPI/libraries/armem_robot/robot_conversions.h
similarity index 89%
rename from source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h
rename to source/RobotAPI/libraries/armem_robot/robot_conversions.h
index ab29163c347bca6cfa78aa1b10581955140a3fc1..3692ae048dacb94d2f24469f5265e998ab23fb34 100644
--- a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.h
@@ -9,7 +9,7 @@ namespace armarx::armem::wm
class EntityInstance;
}
-namespace armarx::armem::articulated_object
+namespace armarx::armem::robot
{
std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..03c8ae1bfed67fb900f50905caeee18ca668a835
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#include <map>
+#include <vector>
+#include <filesystem>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+
+namespace armarx::armem::robot
+{
+ struct RobotDescription
+ {
+ // IceUtil::Time timestamp;
+
+ std::string name;
+ PackagePath xml{"", std::filesystem::path("")};
+ };
+
+ struct RobotState
+ {
+ IceUtil::Time timestamp;
+
+ Eigen::Affine3f globalPose;
+ std::map<std::string, float> jointMap;
+ };
+
+ struct Robot
+ {
+ RobotDescription description;
+ std::string instance;
+
+ RobotState config;
+
+ IceUtil::Time timestamp;
+ };
+
+ using Robots = std::vector<Robot>;
+ using RobotDescriptions = std::vector<RobotDescription>;
+ using RobotStates = std::vector<RobotState>;
+
+} // namespace armarx::armem::robot
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt
deleted file mode 100644
index 24caa9df3f797dc1e259f2709a995d50e1a584a7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt
+++ /dev/null
@@ -1,42 +0,0 @@
-set(LIB_NAME armem_robot_localization)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
-armarx_add_library(
- LIBS
- # ArmarX
- ArmarXCore
- ArmarXCoreInterfaces
- ArmarXGuiComponentPlugins
- # This package
- RobotAPICore
- RobotAPIInterfaces
- RobotAPI::libraries::armem
- # System / External
- Eigen3::Eigen
- HEADERS
- ./TransformInterfaces.h
- ./TransformReader.h
- ./TransformWriter.h
- ./aron_conversions.h
- SOURCES
- ./TransformReader.cpp
- ./TransformWriter.cpp
- ./aron_conversions.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/TransformHeader.xml
- aron/Transform.xml
-)
-
-
-add_library(RobotAPI::armem_robot_localization ALIAS armem_robot_localization)
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h b/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
deleted file mode 100644
index 6962593814414267635efc425637266c7fbcd34a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <Eigen/Geometry>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-namespace armarx
-{
-
- struct TransformHeader
- {
- std::string parentFrame;
- std::string frame;
-
- std::string agent;
-
- std::int64_t timestamp; // [µs]
- };
-
- struct Transform
- {
- TransformHeader header;
-
- Eigen::Affine3f transform = Eigen::Affine3f::Identity();
- };
-
- namespace armem
- {
-
- struct TransformResult
- {
- Transform transform;
-
- enum class Status
- {
- Success,
- ErrorLookupIntoFuture,
- ErrorFrameNotAvailable
- } status;
-
- explicit operator bool() const
- {
- return status == Status::Success;
- }
-
- std::string errorMessage = "";
- };
-
- struct TransformQuery
- {
- TransformHeader header;
-
- // bool exact;
- };
-
- class TransformInterface
- {
- public:
- virtual ~TransformInterface() = default;
-
- virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
- virtual void connect() = 0;
- };
-
- class TransformReaderInterface : virtual public TransformInterface
- {
- public:
- virtual ~TransformReaderInterface() = default;
-
- virtual TransformResult
- getGlobalPose(const std::string& agentName, const std::string& robotRootFrame,
- const std::int64_t& timestamp) const = 0;
-
- virtual TransformResult
- lookupTransform(const TransformQuery& query) const = 0;
- // waitForTransform()
- };
-
- class TransformWriterInterface : virtual public TransformInterface
- {
- public:
- ~TransformWriterInterface() override = default;
-
- virtual bool commitTransform(const Transform& transform) = 0;
- };
-
- } // namespace armem
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp
deleted file mode 100644
index 40832ae90e2686d76a1d2256b8e427c2cafc88a2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-#include "aron_conversions.h"
-
-// STL
-#include <string>
-
-// Ice
-#include <IceUtil/Time.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
-
-
-namespace armarx
-{
-
- TransformHeader fromAron(const aron::TransformHeader& aronHeader)
- {
- TransformHeader header;
-
- header.parentFrame = aronHeader.parentFrame;
- header.frame = aronHeader.frame;
- header.agent = aronHeader.agent;
- header.timestamp = aronHeader.timestamp;
-
- return header;
- }
-
-
- void fromAron(const aron::Transform& aronTransform, Transform& transform)
- {
- transform.header = fromAron(aronTransform.header);
- transform.transform = aronTransform.transform;
- }
-
-
- aron::TransformHeader toAron(const TransformHeader& header)
- {
- aron::TransformHeader aronHeader;
-
- aronHeader.parentFrame = header.parentFrame;
- aronHeader.frame = header.frame;
- aronHeader.agent = header.agent;
- aronHeader.timestamp = header.timestamp;
-
- return aronHeader;
- }
-
- void toAron(const Transform& transform, aron::Transform& aronTransform)
- {
- aronTransform.header = toAron(transform.header);
- aronTransform.transform = transform.transform.matrix();
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
deleted file mode 100644
index 6a5379fe759f73fd03ad7e274a9a2211ba89d846..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#pragma once
-
-
-namespace armarx
-{
- struct Transform;
-
- namespace aron
- {
- struct Transform;
- } // namespace aron
-
- void fromAron(const aron::Transform& aronTransform, Transform& transform);
- void toAron(const Transform& transform, aron::Transform& aronTransform);
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
index 28e32f85f987b88246265ae4fc165c7bd68968de..145ced819b68db659a92e232747c645c24037ce5 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
@@ -9,7 +9,7 @@ armarx_add_library(
ArmarXCore
# This package
RobotAPICore
- RobotAPI::libraries::armem
+ RobotAPI::armem
# System / External
Eigen3::Eigen
HEADERS
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..595bae1a9be089978e681f4dce4dfbd53b8e5ecc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -0,0 +1,68 @@
+set(LIB_NAME armem_robot_state)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ ArmarXCoreInterfaces
+ ArmarXGuiComponentPlugins
+ # This package
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPI::armem
+ RobotAPI::armem_robot
+ # System / External
+ Eigen3::Eigen
+ aroncommon
+ HEADERS
+
+ ./client/localization/interfaces.h
+ ./client/localization/TransformReader.h
+ ./client/localization/TransformWriter.h
+
+ ./server/common/Visu.h
+
+ ./server/localization/Segment.h
+ # ./server/localization/Visu.h
+
+ ./server/proprioception/Segment.h
+ # ./server/proprioception/Visu.h
+
+ ./server/description/Segment.h
+
+
+ ./aron_conversions.h
+ SOURCES
+ ./client/localization/TransformReader.cpp
+ ./client/localization/TransformWriter.cpp
+
+ ./server/common/Visu.cpp
+
+ ./server/localization/Segment.cpp
+ # ./server/localization/Visu.cpp
+
+ ./server/proprioception/Segment.cpp
+ # ./server/proprioception/Visu.cpp
+
+ ./server/description/Segment.cpp
+
+ ./aron_conversions.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/TransformHeader.xml
+ aron/Transform.xml
+)
+
+
+add_library(RobotAPI::armem_robot_state ALIAS armem_robot_state)
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
similarity index 60%
rename from source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
index afdd4fafdbd74b08c19bbb98cbc1dd326ee6244e..02ebe91b35094809953de6cfd0252c8a2048cf98 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
@@ -3,15 +3,15 @@
<AronTypeDefinition>
<CodeIncludes>
<Include include="<Eigen/Core>" />
- <Include include="<RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem_robot_state/aron/TransformHeader.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
- <Include include="<RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml>" />
+ <Include include="<RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml>" />
</AronIncludes>
<GenerateTypes>
- <Object name="armarx::aron::Transform">
+ <Object name="armarx::armem::arondto::Transform">
<ObjectChild key='header'>
- <armarx::aron::TransformHeader />
+ <armarx::armem::arondto::TransformHeader />
</ObjectChild>
<ObjectChild key='transform'>
<Pose />
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml b/source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
similarity index 91%
rename from source/RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
index 85add64bec4441b3e203a2d798322629521ea4fe..c6bd5b94f93fa10e0638c75d17e8dbed0ad5bc60 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
@@ -5,7 +5,7 @@
<Include include="<Eigen/Core>" />
</CodeIncludes>
<GenerateTypes>
- <Object name="armarx::aron::TransformHeader">
+ <Object name="armarx::armem::arondto::TransformHeader">
<ObjectChild key='parentFrame'>
<string />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbca6711774de97e3d137901338fde776e6f2285
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -0,0 +1,47 @@
+#include "aron_conversions.h"
+
+// STL
+#include <string>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// RobotAPI
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include "RobotAPI/libraries/armem_robot_state/types.h"
+
+namespace armarx::armem
+{
+
+ void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ {
+ fromAron(dto.header, bo.header);
+ aron::fromAron(dto.transform, bo.transform);
+ }
+
+ void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ {
+ toAron(dto.header, bo.header);
+ aron::toAron(dto.transform, bo.transform);
+ }
+
+ void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ {
+ aron::toAron(dto.parentFrame, bo.parentFrame);
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.agent, bo.agent);
+ aron::toAron(dto.timestamp, bo.timestamp);
+ }
+
+ void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ {
+ aron::fromAron(dto.parentFrame, bo.parentFrame);
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.agent, bo.agent);
+ aron::fromAron(dto.timestamp, bo.timestamp);
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..d266717dc729e8a09e4f96c3469eb96af38f347d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -0,0 +1,24 @@
+#pragma once
+
+
+namespace armarx::armem
+{
+ namespace robot_state
+ {
+ struct Transform;
+ struct TransformHeader;
+ } // namespace robot_state
+
+ namespace arondto
+ {
+ struct Transform;
+ struct TransformHeader;
+ } // namespace arondto
+
+ void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
+ void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
+
+ void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
+ void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
similarity index 94%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index c3b24c4895328e951bc4950339639200206a9daf..f82bed097130b9b51608ce022424ad99d1481046 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -53,10 +53,10 @@
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-namespace armarx::armem
+namespace armarx::armem::client::robot_state::localization
{
TransformReader::TransformReader(ManagedIceObject& component) : MemoryConnector(component) {}
@@ -202,21 +202,21 @@ namespace armarx::armem
return chain;
}
- inline Transform convertEntityToTransform(const armem::wm::EntityInstance& item)
+ inline ::armarx::armem::robot_state::Transform convertEntityToTransform(const armem::wm::EntityInstance& item)
{
- aron::Transform aronTransform;
+ arondto::Transform aronTransform;
aronTransform.fromAron(item.data());
- Transform transform;
+ ::armarx::armem::robot_state::Transform transform;
fromAron(aronTransform, transform);
return transform;
}
- decltype(auto) findFirstElementAfter(const std::vector<Transform>& transforms,
+ auto findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
const int64_t timestamp)
{
- auto timestampBeyond = [timestamp](const Transform & transform)
+ auto timestampBeyond = [timestamp](const ::armarx::armem::robot_state::Transform & transform)
{
return transform.header.timestamp > timestamp;
};
@@ -225,7 +225,7 @@ namespace armarx::armem
return poseNextIt;
}
- Eigen::Affine3f interpolateTransform(const std::vector<Transform>& queue, int64_t timestamp)
+ Eigen::Affine3f interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue, int64_t timestamp)
{
ARMARX_TRACE;
@@ -288,7 +288,7 @@ namespace armarx::armem
// }
- std::vector<Transform> transforms;
+ std::vector<::armarx::armem::robot_state::Transform> transforms;
transforms.reserve(entity.history().size());
const auto entitySnapshots = simox::alg::get_values(entity.history());
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
similarity index 86%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 4107ed61fcc108737049f6fb063929012575a898..0bad1527d3240e5c0e3f53dbd76f323d67785d03 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -25,9 +25,9 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/MemoryConnector.h>
-#include "TransformInterfaces.h"
+#include "interfaces.h"
-namespace armarx::armem
+namespace armarx::armem::client::robot_state::localization
{
/**
* @defgroup Component-ExampleClient ExampleClient
@@ -41,7 +41,7 @@ namespace armarx::armem
* Detailed description of class ExampleClient.
*/
class TransformReader :
- virtual public ::armarx::armem::TransformReaderInterface,
+ virtual public TransformReaderInterface,
virtual public ::armarx::armem::MemoryConnector
{
public:
@@ -54,6 +54,7 @@ namespace armarx::armem
TransformResult getGlobalPose(const std::string& agentName,
const std::string& robotRootFrame,
const std::int64_t& timestamp) const override;
+
TransformResult lookupTransform(const TransformQuery& query) const override;
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
@@ -71,7 +72,11 @@ namespace armarx::armem
const std::vector<std::string>& tfChain,
const std::string& agent, const std::int64_t& timestamp) const;
- Eigen::Affine3f obtainTransform(const std::string& entityName, const armem::wm::ProviderSegment& agentProviderSegment, int64_t timestamp) const;
+ Eigen::Affine3f obtainTransform(
+ const std::string& entityName,
+ const armem::wm::ProviderSegment& agentProviderSegment,
+ int64_t timestamp)
+ const;
armem::client::Reader memoryReader;
@@ -86,4 +91,4 @@ namespace armarx::armem
const std::string propertyPrefix = "mem.localization.read.";
};
-} // namespace armarx::armem
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
similarity index 90%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 7a9bcd1f4825c6d775201fb862e5a21f0af70cbf..02f567ef0691796c1afd6b69bb2085e861c3022c 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -44,12 +44,11 @@
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include "aron_conversions.h"
-
-namespace armarx::armem
+namespace armarx::armem::client::robot_state::localization
{
TransformWriter::TransformWriter(ManagedIceObject& component) : MemoryConnector(component) {}
@@ -88,7 +87,7 @@ namespace armarx::armem
memoryWriter.setWritingMemory(result.proxy);
}
- bool TransformWriter::commitTransform(const Transform& transform)
+ bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
@@ -114,8 +113,8 @@ namespace armarx::armem
armem::EntityUpdate update;
update.entityID = entityID;
- aron::Transform aronTransform;
- toAron(transform, aronTransform);
+ arondto::Transform aronTransform;
+ toAron(aronTransform, transform);
update.instancesData = {aronTransform.toAron()};
update.timeCreated = IceUtil::Time::microSeconds(aronTransform.header.timestamp);
@@ -139,4 +138,4 @@ namespace armarx::armem
}
-} // namespace armarx::armem
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
similarity index 88%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 7d1ee2acbac7739ea73f92ca2a5205068cf0e5fa..1ea4a3f159da6237965fd5e274d17c9cfeac6561 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -27,9 +27,9 @@
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/MemoryConnector.h>
-#include "TransformInterfaces.h"
+#include "interfaces.h"
-namespace armarx::armem
+namespace armarx::armem::client::robot_state::localization
{
/**
@@ -44,7 +44,7 @@ namespace armarx::armem
* Detailed description of class ExampleClient.
*/
class TransformWriter :
- virtual public ::armarx::armem::TransformWriterInterface,
+ virtual public TransformWriterInterface,
virtual public ::armarx::armem::MemoryConnector
{
public:
@@ -59,7 +59,7 @@ namespace armarx::armem
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
- bool commitTransform(const Transform& transform) override;
+ bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
const std::string& getPropertyPrefix() const override;
@@ -77,4 +77,5 @@ namespace armarx::armem
const std::string propertyPrefix = "mem.localization.write.";
};
-} // namespace armarx::armem
+
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1989d6f6c3289db0f731cebcfede34556153821
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -0,0 +1,86 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::client::robot_state::localization
+{
+ struct TransformResult
+ {
+ ::armarx::armem::robot_state::Transform transform;
+
+ enum class Status
+ {
+ Success,
+ ErrorLookupIntoFuture,
+ ErrorFrameNotAvailable
+ } status;
+
+ explicit operator bool() const { return status == Status::Success; }
+
+ std::string errorMessage = "";
+ };
+
+ struct TransformQuery
+ {
+ ::armarx::armem::robot_state::TransformHeader header;
+
+ // bool exact;
+ };
+
+ class TransformInterface
+ {
+ public:
+ virtual ~TransformInterface() = default;
+
+ virtual void registerPropertyDefinitions(PropertyDefinitionsPtr &def) = 0;
+ virtual void connect() = 0;
+ };
+
+ class TransformReaderInterface : virtual public TransformInterface
+ {
+ public:
+ virtual ~TransformReaderInterface() = default;
+
+ virtual TransformResult getGlobalPose(const std::string &agentName,
+ const std::string &robotRootFrame,
+ const std::int64_t ×tamp) const = 0;
+
+ virtual TransformResult lookupTransform(const TransformQuery &query) const = 0;
+ // waitForTransform()
+ };
+
+ class TransformWriterInterface : virtual public TransformInterface
+ {
+ public:
+ ~TransformWriterInterface() override = default;
+
+ virtual bool commitTransform(const ::armarx::armem::robot_state::Transform &transform) = 0;
+ };
+
+} // namespace armarx::armem::client::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2171928dc1c7675b5dc2596b70560feac98ee3a0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -0,0 +1,232 @@
+#include "Visu.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/CycleUtil.h"
+
+#include <Eigen/src/Geometry/Transform.h>
+#include <IceUtil/Time.h>
+#include <algorithm>
+
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include "RobotAPI/libraries/armem/core/Time.h"
+
+#include "../localization/Segment.h"
+#include "../proprioception/Segment.h"
+
+namespace armarx::armem::server::robot_state
+{
+
+ void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.enabled, prefix + "enabled",
+ "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz",
+ "Frequency of visualization.");
+ }
+
+ void Visu::visualizeRobots(
+ viz::Layer& layer,
+ const robot::Robots& robots
+ ) const
+ {
+
+ ARMARX_INFO << "Entering visualizeObjects";
+ const auto visualizeRobot = [&](const robot::Robot & robot)
+ {
+
+ const auto xmlPath = robot.description.xml.serialize();
+
+ // clang-format off
+ auto robotVisu = viz::Robot(robot.description.name)
+ // .file(xmlPath.package, xmlPath.path)
+ .file("ArmarXObjects", "./data/ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml")
+ .joints(robot.config.jointMap)
+ .pose(robot.config.globalPose);
+
+ robotVisu.useFullModel();
+ // clang-format on
+
+ layer.add(robotVisu);
+ };
+
+ std::for_each(robots.begin(), robots.end(), visualizeRobot);
+ ARMARX_INFO << "Done visualizeRobots";
+
+ }
+
+ void Visu::init()
+ {
+ updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ updateTask->start();
+ }
+
+
+ // void Visu::RemoteGui::setup(const Visu& visu)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // enabled.setValue(visu.enabled);
+ // inGlobalFrame.setValue(visu.inGlobalFrame);
+ // alpha.setRange(0, 1.0);
+ // alpha.setValue(visu.alpha);
+ // alphaByConfidence.setValue(visu.alphaByConfidence);
+ // oobbs.setValue(visu.oobbs);
+ // objectFrames.setValue(visu.objectFrames);
+ // {
+ // float max = 10000;
+ // objectFramesScale.setRange(0, max);
+ // objectFramesScale.setDecimals(2);
+ // objectFramesScale.setSteps(int(10 * max));
+ // objectFramesScale.setValue(visu.objectFramesScale);
+ // }
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ // row++;
+ // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
+ // row++;
+ // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
+ // row++;
+ // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ // row++;
+ // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
+ // row++;
+ // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
+ // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
+ // row++;
+
+ // group.setLabel("Visualization");
+ // group.addChild(grid);
+ // }
+
+ // void Visu::RemoteGui::update(Visu& visu)
+ // {
+ // visu.enabled = enabled.getValue();
+ // visu.inGlobalFrame = inGlobalFrame.getValue();
+ // visu.alpha = alpha.getValue();
+ // visu.alphaByConfidence = alphaByConfidence.getValue();
+ // visu.oobbs = oobbs.getValue();
+ // visu.objectFrames = objectFrames.getValue();
+ // visu.objectFramesScale = objectFramesScale.getValue();
+ // }
+
+ robot::Robots combine(const robot::RobotDescriptions& robotDescriptions,
+ const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
+ const std::unordered_map<std::string, std::map<std::string, float>>& robotJointPositionMap)
+ {
+
+ robot::Robots robots;
+
+ for(const auto& robotDescription: robotDescriptions)
+ {
+ const auto& robotName = robotDescription.name;
+
+ const auto& globalPose = globalRobotPoseMap.find(robotName);
+ if(globalPose == globalRobotPoseMap.end())
+ {
+ ARMARX_WARNING << "No global pose for robot '" << robotName << "'";
+ continue;
+ }
+
+ const auto& jointMap = robotJointPositionMap.find(robotName);
+ if(jointMap == robotJointPositionMap.end())
+ {
+ ARMARX_WARNING << "No joint positions for robot '" << robotName << "'";
+ continue;
+ }
+
+ // TODO(fabian.reister): based on data
+ const armem::Time timestamp = IceUtil::Time::now();
+
+ robots.emplace_back(
+ robot::Robot{
+ .description = robotDescription,
+ .instance = "", // TODO(fabian.reister): set this properly
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = globalPose->second,
+ .jointMap = jointMap->second
+ },
+ .timestamp = timestamp,
+ }
+ );
+ }
+
+ return robots;
+
+ }
+
+
+ void Visu::visualizeRun()
+ {
+
+ // TODO need another core segment
+ const robot::RobotDescriptions robotDescriptions
+ {
+ {
+ robot::RobotDescription{
+ .name = "Armar6",
+ .xml = {"Armar6RT", "./Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml"}
+ }
+ }
+ };
+
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask && not updateTask->isStopped())
+ {
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_IMPORTANT << "Update task";
+
+ if (p.enabled)
+ {
+ // TIMING_START(Visu);
+
+ const auto globalRobotPoseMap = localizationSegment.getRobotGlobalPoses();
+ const auto robotJointPositionMap = proprioceptionSegment.getRobotJointPositions();
+
+ // we need all 3 informations:
+ // - robot description
+ // - global pose
+ // - joint positions
+ // => this is nothing but a armem::Robot
+
+ const robot::Robots robots = combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
+ viz::Layer layer = arviz.layer("Robots");
+
+ ARMARX_INFO << "visualizing robots";
+ visualizeRobots(layer, robots);
+
+ ARMARX_INFO << "Committing robots";
+
+ arviz.commit({layer});
+
+ ARMARX_INFO << "Done committing";
+
+
+ // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..4297b9ccfd865805ea99b972c5db540872a73702
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -0,0 +1,115 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+
+namespace armarx
+{
+ class ObjectFinder;
+}
+
+namespace armarx::armem::server::robot_state
+{
+ namespace localization
+ {
+ class Segment;
+ }
+
+ namespace proprioception
+ {
+ class Segment;
+ }
+
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+
+ Visu(const viz::Client& arviz,
+ const proprioception::Segment& proprioceptionSegment,
+ const localization::Segment& localizationSegment)
+ : arviz(arviz),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
+ {}
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+
+ void init();
+
+ protected:
+
+ void visualizeRobots(
+ viz::Layer& layer,
+ const robot::Robots& robots
+ ) const;
+
+
+ private:
+ viz::Client arviz;
+
+ const proprioception::Segment& proprioceptionSegment;
+ const localization::Segment& localizationSegment;
+
+ struct Properties
+ {
+ bool enabled = true;
+ float frequencyHz = 25;
+ } p;
+
+
+ SimpleRunningTask<>::pointer_type updateTask;
+ void visualizeRun();
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::CheckBox enabled;
+
+ // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
+ // armarx::RemoteGui::Client::FloatSlider alpha;
+ // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
+ // armarx::RemoteGui::Client::CheckBox oobbs;
+ // armarx::RemoteGui::Client::CheckBox objectFrames;
+ // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+
+ // // void setup(const Visu& visu);
+ // // void update(Visu& visu);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..20c3a1ec239367eb7606e1ca0e43f12895fdfff0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -0,0 +1,199 @@
+#include "Segment.h"
+
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <sstream>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#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>
+
+namespace armarx::armem::server::robot_state::description
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter &memoryToIceAdapter,
+ std::mutex &memoryMutex) :
+ iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ArticulatedObjectInstanceSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix)
+ {
+ defs->optional(p.coreSegment,
+ prefix + "CoreSegment",
+ "Name of the robot description core segment.");
+ defs->optional(p.maxHistorySize,
+ prefix + "MaxHistorySize",
+ "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(
+ p.coreSegment, arondto::RobotDescription::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(const viz::Client &arviz, const RobotUnitInterfacePrx &robotUnitPrx)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ robotUnit = robotUnitPrx;
+ }
+
+ void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(robotDescription.name);
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+
+ EntityUpdate update;
+ // update.entityID = providerID.withEntityName(info.id().str());
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::RobotDescription dto;
+ robot::toAron(dto, robotDescription);
+ update.instancesData =
+ {
+ dto.toAron()
+ };
+
+ Commit commit;
+ commit.updates.push_back(std::move(update));
+
+ iceMemory.commit(commit);
+ }
+
+ void Segment::updateRobotDescription()
+ {
+ auto kinematicUnit = robotUnit->getKinematicUnit();
+
+ const auto robotName = kinematicUnit->getRobotName();
+
+ const robot::RobotDescription robotDescription{
+ .name = kinematicUnit->getRobotName(),
+ .xml = {"Armar6RT", kinematicUnit->getRobotFilename()}};
+
+ storeRobotDescription(robotDescription);
+ }
+
+ Segment::RobotDescriptionMap Segment::getRobotDescriptions() const
+ {
+ RobotDescriptionMap robotDescriptions;
+
+ for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto &[name, entity] : provSeg.entities())
+ {
+ const auto &entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto description = robot::convertRobotDescription(entityInstance);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to "
+ "'RobotDescription'";
+ continue;
+ }
+
+ ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ robotDescriptions.emplace(description->name, *description);
+ }
+ }
+
+ ARMARX_IMPORTANT << "Number of known robot descriptions: " << robotDescriptions.size();
+
+ return robotDescriptions;
+ }
+
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ // {
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ // {
+ // for (const auto& [name, entity] : provSeg.entities())
+ // {
+ // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ // const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ // if (not description)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ // continue;
+ // }
+
+ // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ // objects.emplace(armem::MemoryID(entity.id()), *description);
+ // }
+ // }
+
+ // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+ // return objects;
+ // }
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..8bb9ec017b006c1ad9dc5334688d04aa472e9ee1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -0,0 +1,114 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::description
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ /// mapping "robot name" -> "robot description"
+ using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
+
+ RobotDescriptionMap getRobotDescriptions() const;
+
+ private:
+
+ void storeRobotDescription(const robot::RobotDescription& robotDescription);
+ void updateRobotDescription();
+
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ RobotUnitInterfacePrx robotUnit;
+
+ struct Properties
+ {
+ std::string coreSegment = "Description";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..180b9aece6de2e1ef39e5c58e5202d0999f05e72
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -0,0 +1,139 @@
+#include "Segment.h"
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
+
+namespace armarx::armem::server::robot_state::localization
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ArticulatedObjectInstanceSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::Transform::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+ std::unordered_map<std::string, Eigen::Affine3f> Segment::getRobotGlobalPoses() const
+ {
+ std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
+
+ // TODO(fabian.reister): implement
+
+ return robotGlobalPoses;
+ }
+
+
+
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ // {
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ // {
+ // for (const auto& [name, entity] : provSeg.entities())
+ // {
+ // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ // const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ // if (not description)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ // continue;
+ // }
+
+ // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ // objects.emplace(armem::MemoryID(entity.id()), *description);
+ // }
+ // }
+
+ // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+ // return objects;
+ // }
+
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..465ba2d8bba58bb7408f06abf13120b0a6727bc2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -0,0 +1,104 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::localization
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses() const;
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ struct Properties
+ {
+ std::string coreSegment = "Localization";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..29bd6d31db98b2779369858717aa87c5a72711aa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.cpp
@@ -0,0 +1,172 @@
+#include "Visu.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/CycleUtil.h"
+
+#include <algorithm>
+
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include "Segment.h"
+
+namespace armarx::armem::server::obj::articulated_object_instance
+{
+
+ void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.enabled, prefix + "enabled",
+ "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz",
+ "Frequency of visualization.");
+ }
+
+
+ viz::Layer Visu::visualizeProvider(
+ const std::string& providerName,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const
+ {
+ viz::Layer layer = arviz.layer(providerName);
+
+ visualizeRobots(layer, objects);
+
+ return layer;
+ }
+
+
+ void Visu::visualizeR(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
+ {
+ ARMARX_INFO << "Entering visualizeObjects";
+ const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
+ {
+
+ const auto xmlPath = obj.description.xml.serialize();
+
+ // clang-format off
+ auto robot = viz::Robot(obj.description.name)
+ // .file(xmlPath.package, xmlPath.path)
+ .file("ArmarXObjects", "./data/ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml")
+ .joints(obj.config.jointMap)
+ .pose(obj.config.globalPose);
+
+ robot.useFullModel();
+ // clang-format on
+
+ layer.add(robot);
+ };
+
+ std::for_each(objects.begin(), objects.end(), visualizeObject);
+ ARMARX_INFO << "Done visualizeObjects";
+
+ }
+
+ void Visu::init()
+ {
+ updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ updateTask->start();
+ }
+
+
+ // void Visu::RemoteGui::setup(const Visu& visu)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // enabled.setValue(visu.enabled);
+ // inGlobalFrame.setValue(visu.inGlobalFrame);
+ // alpha.setRange(0, 1.0);
+ // alpha.setValue(visu.alpha);
+ // alphaByConfidence.setValue(visu.alphaByConfidence);
+ // oobbs.setValue(visu.oobbs);
+ // objectFrames.setValue(visu.objectFrames);
+ // {
+ // float max = 10000;
+ // objectFramesScale.setRange(0, max);
+ // objectFramesScale.setDecimals(2);
+ // objectFramesScale.setSteps(int(10 * max));
+ // objectFramesScale.setValue(visu.objectFramesScale);
+ // }
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ // row++;
+ // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
+ // row++;
+ // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
+ // row++;
+ // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ // row++;
+ // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
+ // row++;
+ // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
+ // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
+ // row++;
+
+ // group.setLabel("Visualization");
+ // group.addChild(grid);
+ // }
+
+ // void Visu::RemoteGui::update(Visu& visu)
+ // {
+ // visu.enabled = enabled.getValue();
+ // visu.inGlobalFrame = inGlobalFrame.getValue();
+ // visu.alpha = alpha.getValue();
+ // visu.alphaByConfidence = alphaByConfidence.getValue();
+ // visu.oobbs = oobbs.getValue();
+ // visu.objectFrames = objectFrames.getValue();
+ // visu.objectFramesScale = objectFramesScale.getValue();
+ // }
+
+
+ void Visu::visualizeRun()
+ {
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask && not updateTask->isStopped())
+ {
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_IMPORTANT << "Update task";
+
+ if (p.enabled)
+ {
+ // TIMING_START(Visu);
+
+ const auto articulatedObjects = segment.getArticulatedObjects();
+
+ ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
+
+ viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
+
+ ARMARX_INFO << "visualizing objects";
+ visualizeObjects(layer, articulatedObjects);
+
+ ARMARX_INFO << "Committing objects";
+
+ arviz.commit({layer});
+
+ ARMARX_INFO << "Done committing";
+
+
+ // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..54aea5e09f0d3000596c83d2eb8ffbafcbf8b81e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.h
@@ -0,0 +1,101 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+
+namespace armarx
+{
+ class ObjectFinder;
+}
+namespace armarx::armem::server::obj::articulated_object_instance
+{
+ class Segment;
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+
+ Visu(const viz::Client& arviz, const Segment& segment): arviz(arviz), segment(segment) {}
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+
+ void init();
+
+ protected:
+ viz::Layer visualizeProvider(
+ const std::string& providerName,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects
+ ) const;
+
+ void visualizeObjects(
+ viz::Layer& layer,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects
+ ) const;
+
+
+ private:
+ viz::Client arviz;
+ const Segment& segment;
+
+ struct Properties
+ {
+ bool enabled = true;
+ float frequencyHz = 25;
+ } p;
+
+
+ SimpleRunningTask<>::pointer_type updateTask;
+ void visualizeRun();
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::CheckBox enabled;
+
+ // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
+ // armarx::RemoteGui::Client::FloatSlider alpha;
+ // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
+ // armarx::RemoteGui::Client::CheckBox oobbs;
+ // armarx::RemoteGui::Client::CheckBox objectFrames;
+ // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+
+ // // void setup(const Visu& visu);
+ // // void update(Visu& visu);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..efedbbe42eeaf740a87a43dff5c86364a8797a27
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -0,0 +1,165 @@
+#include "Segment.h"
+
+#include <mutex>
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ArticulatedObjectInstanceSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment); // TODO , arondto::Robot::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx)
+ {
+ robotUnit = robotUnitPrx;
+
+ auto kinematicUnit = robotUnit->getKinematicUnit();
+ const auto providerSegmentName = kinematicUnit->getRobotName();
+
+ // TODO what is the purpose?
+ auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
+ auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
+ auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
+ encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
+ encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
+ //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
+ //encoderEntryType->addMemberType("value", encoderValueType);
+
+ ARMARX_INFO << "Adding provider segment " << p.coreSegment << "/" << providerSegmentName;
+ armem::data::AddSegmentInput input;
+ input.coreSegmentName = p.coreSegment;
+ input.providerSegmentName = providerSegmentName;
+
+ {
+ std::lock_guard g{memoryMutex};
+ auto result = iceMemory.addSegments({input})[0];
+
+ if (!result.success)
+ {
+ ARMARX_ERROR << "Could not add segment " << p.coreSegment << "/" << providerSegmentName << ". The error message is: " << result.errorMessage;
+ }
+ }
+
+ robotUnitProviderID.memoryName = iceMemory.workingMemory->id().memoryName;
+ robotUnitProviderID.coreSegmentName = p.coreSegment;
+ robotUnitProviderID.providerSegmentName = providerSegmentName;
+
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+ std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions() const
+ {
+ std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+
+ // for (const auto& [provName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ // {
+ // for (const auto& [entityName, entity] : provSeg.entities())
+ // {
+ // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ // const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ // if (not description)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ // continue;
+ // }
+
+ // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ // objects.emplace(armem::MemoryID(entity.id()), *description);
+ // }
+ // }
+
+ return jointMap;
+ }
+
+ const armem::MemoryID& Segment::getRobotUnitProviderID() const
+ {
+ return robotUnitProviderID;
+ }
+
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..09bae1077a76757fcce175ec34dff212f6c4d2f3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -0,0 +1,110 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions() const;
+
+ const armem::MemoryID& getRobotUnitProviderID() const;
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ RobotUnitInterfacePrx robotUnit;
+
+ armem::MemoryID robotUnitProviderID;
+
+ struct Properties
+ {
+ std::string coreSegment = "Proprioception";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..3edaee80f8696d844f1f5c90b299c0a54c503f93
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+namespace armarx::armem::robot_state
+{
+ struct TransformHeader
+ {
+ std::string parentFrame;
+ std::string frame;
+
+ std::string agent;
+
+ std::int64_t timestamp; // [µs]
+ };
+
+ struct Transform
+ {
+ TransformHeader header;
+
+ Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 6fc9fa463f3f037c38f6e0142c934678775d9aaa..ec6c74d9e669f9effb5825afda1cc1645aadf547 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -17,12 +17,14 @@ armarx_add_library(
aron_conversions/armarx.h
aron_conversions/simox.h
aron_conversions/stl.h
+ aron_conversions/eigen.h
SOURCES
aron_conversions/core.cpp
aron_conversions/armarx.cpp
aron_conversions/simox.cpp
aron_conversions/stl.cpp
+ aron_conversions/eigen.cpp
)
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions.h b/source/RobotAPI/libraries/aron/common/aron_conversions.h
index 2f96d0285b9fb4dbd96abdb92c12fe9518ee88b6..b771ef362aaf0a886aaabcc19027e74a9dfbeb9b 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions.h
@@ -5,3 +5,4 @@
#include "aron_conversions/armarx.h"
#include "aron_conversions/simox.h"
#include "aron_conversions/stl.h"
+#include "aron_conversions/eigen.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0bf14ec90439e50a4869234b81449adaf068d173
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
@@ -0,0 +1,17 @@
+#include "eigen.h"
+
+namespace armarx::aron
+{
+ using AronPose = Eigen::Matrix<float, 4, 4>;
+
+ void fromAron(const AronPose& dto, Eigen::Affine3f& bo)
+ {
+ bo.matrix() = dto;
+ }
+
+ void toAron(AronPose& dto, const Eigen::Affine3f& bo)
+ {
+ dto = bo.matrix();
+ }
+
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f091dbfc40ee8e91b8d2922687273269ce790a8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+namespace armarx::aron
+{
+ using AronPose = Eigen::Matrix<float, 4, 4>;
+
+ void fromAron(const AronPose& dto, Eigen::Affine3f& bo);
+ void toAron(AronPose& dto, const Eigen::Affine3f& bo);
+
+} // namespace armarx