diff --git a/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
index 4eb4b16a3d8df576ee0a2a870aaced1f69a78816..37675a4f29325e9d3da5e55be23c6a52d66f5aa4 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
+++ b/source/RobotAPI/components/DSObstacleAvoidance/CMakeLists.txt
@@ -11,9 +11,6 @@ endif()
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
-if(Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIR})
-endif()
find_package(PythonLibs 3.6 QUIET)
armarx_build_if(PYTHONLIBS_FOUND "Python libs not available")
@@ -26,6 +23,7 @@ set(COMPONENT_LIBS
RobotAPIComponentPlugins
RobotAPIInterfaces
dynamic_obstacle_avoidance
+ Simox::SimoxUtility
)
set(SOURCES DSObstacleAvoidance.cpp)
diff --git a/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
index 7136f05db5538c47faa2ed7d875a69166172e6db..abcd773ce86a236dcd2f15bdf3e3fe1d22718017 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
+++ b/source/RobotAPI/components/DynamicObstacleManager/CMakeLists.txt
@@ -3,15 +3,9 @@ armarx_component_set_name("DynamicObstacleManager")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-if(Eigen3_FOUND)
- include_directories(${Eigen3_INCLUDE_DIR})
-endif()
find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
armarx_build_if(Simox_FOUND "Simox ${ArmarX_Simox_VERSION} or later not available")
-if (Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
-endif()
find_package(OpenCV QUIET)
armarx_build_if(OpenCV_FOUND "OpenCV not available")
@@ -25,8 +19,10 @@ set(COMPONENT_LIBS
RobotAPICore
RobotAPIComponentPlugins
RobotAPIInterfaces
+ Eigen3::Eigen
${OpenCV_LIBS}
- ${Simox_LIBS}
+ Simox::VirtualRobot
+ # todo maybe add other simox libs
)
set(SOURCES
@@ -42,4 +38,3 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
armarx_generate_and_add_component_executable()
-
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index c944eda90774140a09c4b2376bcfa734b000817f..fabf44525736293fdbf5f2f84a2bdbef951e2954 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -26,7 +26,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
namespace armarx::armem
{
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
index 23b426f37c9dc8942a621cde3b5746e6cdb52ae3..318ab3d38183f7cf93337082b5b0386c5451f609 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
@@ -38,7 +38,7 @@
#include <RobotAPI/libraries/armem_index/memory_ids.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
index 492c2c14214d5b1a31a8c0998843a5b799ab495b..94fbe1b178a3d538601976da47f28215d8ec30d0 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
@@ -134,7 +134,7 @@ namespace armarx::armem::robot_state
ARMARX_CHECK_EQUAL(localizationPredictionResults.size(), localizationEntityIDs.size());
std::stringstream errorMessages;
- namespace loc = armem::common::robot_state::localization;
+ namespace loc = armem::robot_state::localization;
std::optional<armem::wm::CoreSegment> coreSegment;
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
index a94f44f2160ec2ee4ab67618a8552ffdbdf8ed28..f9f41702e8cde482679fdddf8bd8a91c948b9c54 100644
--- a/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
@@ -8,8 +8,6 @@ set(COMPONENT_LIBS
RobotAPICore RobotAPIInterfaces armem_server
RobotAPIComponentPlugins # for ArViz and other plugins
armem_robot_state
- armem_robot
-
armem_laser_scans
${IVT_LIBRARIES}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 879422d20200f53cc0befe9b1c28f6b59220396a..c1786b00a2ff1b0588c8d740a6e60eb637a9fe32 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -16,6 +16,7 @@ set(LIBS
Eigen3::Eigen
${manif_LIBRARIES}
+ #armem_robot_state_aron
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index aab6379138035bbabb26599742ec74372779fd66..2a3716d2bc3a93aa38764e202f644ae13ab2c3c6 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -27,7 +27,6 @@ add_subdirectory(armem_motions)
add_subdirectory(armem_mps)
add_subdirectory(armem_objects)
add_subdirectory(armem_reasoning)
-add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
add_subdirectory(armem_skills)
add_subdirectory(armem_system_state)
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index e150f1e044f225c73aeda5f330a50ffef1c51a16..f127ebd92200346de7421fdd493443971e984045 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -133,7 +133,7 @@ namespace armarx::armem
{
std::vector<std::string> allItems = this->getAllItems(true);
std::string newID = "";
- for(int i = 0; i < allItems.size(); i++){
+ for(std::size_t i = 0; i < allItems.size(); i++){
if(!allItems.at(i).empty()){
std::string toAppend = allItems.at(i);
if(allItems.at(i).find(delimiter) != std::string::npos){
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
index 2c140c94b5950afdb89c887c78198db57ba52457..00568c75b2d8e69eabd2d56d27a0b99e1744bcd6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
@@ -62,7 +62,7 @@ namespace armarx::armem::server::ltm
std::map<std::string, processor::SnapshotFilter::FilterStatistics> stats;
bool recordedOverall = false;
ARMARX_INFO << "Number of active filters: " << snapFilters.size();
- for (int i = 0; i < snapFilters.size(); i++)
+ for (std::size_t i = 0; i < snapFilters.size(); i++)
{
auto statistics = snapFilters.at(i)->getFilterStatistics();
auto recorded = statistics.accepted + statistics.rejected;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
index 67448db2dba914f0d7de8f392433e55f03725026..e5d3a7b1fac491353895ecfa44fbb9df089b4578 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
@@ -38,7 +38,7 @@ namespace armarx::armem::server::ltm::processor::filter
std::map<MemoryID, long> timestampLastCommitInMs;
std::double_t threshold;
FilterStatistics stats;
- int max_images = 2;
+ std::size_t max_images = 2;
aron::similarity::NDArraySimilarity::Type similarity_type;
aron::similarity::FloatSimilarity::Type float_similarity_type;
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index 6eafef5ed875232aa83ab237f058a3c29db08a60..ab4750e34435213bd5db7541012fe8e21a7bb2bb 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -13,9 +13,9 @@
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.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>
+// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::grasping::known_grasps
diff --git a/source/RobotAPI/libraries/armem_index/CMakeLists.txt b/source/RobotAPI/libraries/armem_index/CMakeLists.txt
index e586cd150e061a690b1d18b672028e17ac85a3fa..d9e28596e6a9155d9a470a3d1b66cf1fd87389ec 100644
--- a/source/RobotAPI/libraries/armem_index/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_index/CMakeLists.txt
@@ -13,7 +13,6 @@ armarx_add_library(
# RobotAPI
RobotAPI::ArViz
RobotAPI::armem
- # RobotAPI::armem_robot
HEADERS
forward_declarations.h
diff --git a/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
index 86a96909df32e14bbc90a95a9e2a600e86181c9f..e6060daf003fff505e848de0963bb5a430fa35e0 100644
--- a/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
@@ -19,9 +19,6 @@ armarx_add_library(
RobotAPI::armem_server
RobotAPI::armem_index
- # armem_robot_state
- # armem_robot
-
HEADERS
server.h
diff --git a/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
index f40102f3bb1c12ea9d6884e58d4115f25634737b..92b7689828131699d3eb6fde64e99994358b4ca2 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
@@ -13,7 +13,6 @@ armarx_add_library(
aroncommon
aronvectorconverter
armem_robot_state
- armem_robot
# System / External
Eigen3::Eigen
HEADERS
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index bbb52cc2b5f473d71cbf1d7c73e39002368f71ad..70eab90df6a473558a51ff94fcdecd5446ba27f1 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -15,7 +15,8 @@ armarx_add_library(
RobotAPI::ComponentPlugins
RobotAPI::Core
RobotAPI::armem
- RobotAPI::armem_robot
+ RobotAPI::armem_robot_state
+# armem_robot_state_aron
HEADERS
aron_conversions.h
@@ -44,6 +45,7 @@ armarx_add_library(
client/articulated_object/ArticulatedObjectWriter.h
client/articulated_object/interfaces.h
client/articulated_object/utils.h
+ client/articulated_object/aron_conversions.h
client/attachment/Reader.h
client/attachment/Writer.h
@@ -63,6 +65,7 @@ armarx_add_library(
client/articulated_object/Writer.cpp
client/articulated_object/ArticulatedObjectReader.cpp
client/articulated_object/ArticulatedObjectWriter.cpp
+ client/articulated_object/aron_conversions.cpp
client/articulated_object/utils.cpp
client/attachment/Reader.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index 093284885bc00e2b50688234c76e94c79795db21..011ee22fafcf6324aa307dbfa5bfb48bf31bb9c5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -17,9 +17,9 @@
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.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>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
namespace armarx::armem::articulated_object
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 5e5b15f15d8135b7de89386c7ba95a7d65f29b9e..ba62496536c4c1d5044ee4c96d84612860dc6a4d 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -2,6 +2,7 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/Transform.h>
#include <VirtualRobot/Robot.h>
@@ -41,7 +42,7 @@ namespace armarx::armem::articulated_object
obj.getFilename())},
.instance = obj.getName(),
.config = {.timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
.jointMap = obj.getJointValues(),
.proprioception = std::nullopt},
.timestamp = timestamp};
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 f21151f1982a28f852bc2b79215709cb0520e4f1..24e2a29d828214237ed8b9114fad4bbffd645762 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -20,13 +20,12 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/constants.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>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include "utils.h"
-
namespace armarx::armem::articulated_object
{
@@ -161,7 +160,7 @@ namespace armarx::armem::articulated_object
return true;
}
- std::vector<robot::RobotDescription>
+ std::vector<robot_state::description::RobotDescription>
Reader::queryDescriptions(const armem::Time& timestamp,
const std::optional<std::string>& providerName) const
{
@@ -205,7 +204,7 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
Reader::queryDescription(const std::string& name,
const armem::Time& timestamp,
const std::optional<std::string>& providerName) const
@@ -252,7 +251,7 @@ namespace armarx::armem::articulated_object
return getRobotDescription(qResult.memory);
}
- std::optional<robot::RobotState>
+ std::optional<robot_state::RobotState>
Reader::queryState(const std::string& instanceName,
const armem::Time& timestamp,
const std::optional<std::string>& providerName) const
@@ -301,7 +300,7 @@ namespace armarx::armem::articulated_object
return getArticulatedObjectState(qResult.memory);
}
- std::optional<robot::RobotState>
+ std::optional<robot_state::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
ARMARX_TRACE;
@@ -320,15 +319,16 @@ namespace armarx::armem::articulated_object
objpose::ObjectPose objectPose;
objpose::fromAron(aronObjectInstance.pose, objectPose);
- robot::RobotState robotState{.timestamp = objectPose.timestamp,
- .globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues,
- .proprioception = std::nullopt};
+ robot_state::RobotState robotState{.timestamp = objectPose.timestamp,
+ .globalPose =
+ Eigen::Isometry3f(objectPose.objectPoseGlobal),
+ .jointMap = objectPose.objectJointValues,
+ .proprioception = std::nullopt};
return robotState;
}
- std::optional<robot::RobotState>
+ std::optional<robot_state::RobotState>
Reader::getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const
{
ARMARX_TRACE;
@@ -350,7 +350,7 @@ namespace armarx::armem::articulated_object
}
}
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
ARMARX_TRACE;
@@ -371,7 +371,7 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- std::vector<robot::RobotDescription>
+ std::vector<robot_state::description::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
ARMARX_TRACE;
@@ -379,7 +379,7 @@ namespace armarx::armem::articulated_object
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(objects::constants::CoreClassSegmentName);
- std::vector<robot::RobotDescription> descriptions;
+ std::vector<robot_state::description::RobotDescription> descriptions;
coreSegment.forEachEntity(
[&descriptions](const wm::Entity& entity)
{
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 50c342c0cfc87614505df230ecdc3fce9cd8b43b..93abc3332a5f0ab32a1fe6a95560d40af35b33ee 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -53,9 +53,10 @@ namespace armarx::armem::articulated_object
void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- [[nodiscard]] bool synchronize(ArticulatedObject& obj,
- const armem::Time& timestamp,
- const std::optional<std::string>& providerName) const override;
+ [[nodiscard]] bool
+ synchronize(ArticulatedObject& obj,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName) const override;
std::optional<ArticulatedObject>
get(const std::string& name,
@@ -66,15 +67,16 @@ namespace armarx::armem::articulated_object
const std::string& instanceName,
const std::optional<std::string>& providerName) const override;
- std::optional<robot::RobotState> queryState(const std::string& instanceName,
- const armem::Time& timestamp,
- const std::optional<std::string>& providerName) const;
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::RobotState>
+ queryState(const std::string& instanceName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName) const;
+ std::optional<robot_state::description::RobotDescription>
queryDescription(const std::string& name,
const armem::Time& timestamp,
const std::optional<std::string>& providerName) const;
- std::vector<robot::RobotDescription>
+ std::vector<robot_state::description::RobotDescription>
queryDescriptions(const armem::Time& timestamp,
const std::optional<std::string>& providerName) const;
@@ -82,11 +84,11 @@ namespace armarx::armem::articulated_object
void setProviderName(const std::string& providerName);
protected:
- std::optional<robot::RobotState>
+ std::optional<robot_state::RobotState>
getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const;
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory) const;
- std::vector<robot::RobotDescription>
+ std::vector<robot_state::description::RobotDescription>
getRobotDescriptions(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 4df7921025ba582a4a913a0fa88906959137b3bb..d061c692e3399b07279a6cd5420c295396d34e38 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -15,10 +15,11 @@
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.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_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include "utils.h"
@@ -274,7 +275,7 @@ namespace armarx::armem::articulated_object
}
// TODO this is a duplicate
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index bf1c7d9459654c422f27c26b8cab9c9e6ae9b20f..dcc49a2b2dc9e9e32a8edd06efa2026e0bb326c6 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -67,7 +67,7 @@ namespace armarx::armem::articulated_object
// TODO duplicate
std::unordered_map<std::string, armem::MemoryID>
queryDescriptions(const armem::Time& timestamp);
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory) const;
std::unordered_map<std::string, armem::MemoryID>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98b84e3f6760181ee05e60091137bc4b31af8815
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.cpp
@@ -0,0 +1,55 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+namespace armarx::armem::robot_state::description
+{
+
+ void
+ fromAron(const arondto::ObjectClass& dto, RobotDescription& bo)
+ {
+ bo.name = aron::fromAron<armarx::ObjectID>(dto.id).str();
+ fromAron(dto.articulatedSimoxXmlPath, bo.xml);
+ }
+
+ void
+ toAron(arondto::ObjectClass& dto, const RobotDescription& bo)
+ {
+ toAron(dto.id, ObjectID(bo.name));
+ toAron(dto.articulatedSimoxXmlPath, bo.xml);
+ }
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+ void
+ fromAron(const arondto::ObjectInstance& dto, RobotState& bo)
+ {
+ fromAron(dto.pose, bo);
+ }
+
+ void
+ toAron(arondto::ObjectInstance& dto, const RobotState& bo)
+ {
+ toAron(dto.pose, bo);
+ }
+
+ void
+ fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
+ {
+ aron::fromAron(dto.timestamp, bo.timestamp);
+ aron::fromAron(dto.objectPoseGlobal, bo.globalPose);
+ aron::fromAron(dto.objectJointValues, bo.jointMap);
+ }
+
+ void
+ toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
+ {
+ aron::toAron(dto.timestamp, bo.timestamp);
+ aron::toAron(dto.objectPoseGlobal, bo.globalPose.matrix());
+ aron::toAron(dto.objectJointValues, bo.jointMap);
+ }
+
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..d0ce1aaa0c3c5f3d44bc251c239da22aead42384
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::robot_state::description
+{
+ void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
+ void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+
+ void fromAron(const arondto::ObjectInstance& dto, RobotState& bo);
+ void toAron(arondto::ObjectInstance& dto, const RobotState& bo);
+
+ void fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo);
+ void toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo);
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
index 11c389be63160c69d449244bf6d125f71e7bc225..4afc5a0196a9715d7cb7efd14d3b4c88e81ccf2b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
@@ -1,12 +1,13 @@
#include "utils.h"
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx::armem::articulated_object
{
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
convertRobotDescription(const armem::wm::EntityInstance& instance)
{
@@ -21,7 +22,7 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- robot::RobotDescription robotDescription;
+ robot_state::description::RobotDescription robotDescription;
fromAron(aronObjectInfo, robotDescription);
// check if robot description is valid
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
index 5bb773c96354b108185811137602bdc443c0f9a7..62ec0132d5200075ffea139890471a233d9a43ff 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
@@ -3,14 +3,13 @@
#include <optional>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::articulated_object
{
- std::optional<robot::RobotDescription>
+ std::optional<robot_state::description::RobotDescription>
convertRobotDescription(const armem::wm::EntityInstance& instance);
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
index 938ba098681aca145efbeb131af9c7a6a1b6c054..c724a3fd78bff4674428dcd4e4d21ac69ffff9a9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -11,9 +11,9 @@
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.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_robot_state/robot_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
index 9a59562ec531ce44644b4bda5ed2a8e9a0c6ac3e..37ad1d6cfe603b749c7d9956b66ff4dd22972f1a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.cpp
@@ -1,29 +1,33 @@
#include "Writer.h"
-#include <IceUtil/Time.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <mutex>
#include <optional>
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.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>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
namespace armarx::armem::attachment
{
Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
- {}
+ {
+ }
- void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -37,7 +41,8 @@ namespace armarx::armem::attachment
def->optional(properties.providerName, prefix + "ProviderName");
}
- void Writer::connect()
+ void
+ Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
@@ -54,12 +59,13 @@ namespace armarx::armem::attachment
}
}
-
- std::optional<armem::MemoryID> Writer::commit(const ObjectAttachment& attachment)
+ std::optional<armem::MemoryID>
+ Writer::commit(const ObjectAttachment& attachment)
{
std::lock_guard g{memoryWriterMutex};
- const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+ const auto result =
+ memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
if (not result.success)
{
@@ -72,8 +78,8 @@ namespace armarx::armem::attachment
const auto providerId = armem::MemoryID(result.segmentID);
const auto entityID =
providerId
- .withEntityName(attachment.object.entityName) // TODO check if meaningful
- .withTimestamp(timestamp);
+ .withEntityName(attachment.object.entityName) // TODO check if meaningful
+ .withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -82,7 +88,7 @@ namespace armarx::armem::attachment
toAron(aronAttachment, attachment);
update.instancesData = {aronAttachment.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -96,14 +102,15 @@ namespace armarx::armem::attachment
}
return updateResult.snapshotID;
-
}
- std::optional<armem::MemoryID> Writer::commit(const ArticulatedObjectAttachment& attachment)
+ std::optional<armem::MemoryID>
+ Writer::commit(const ArticulatedObjectAttachment& attachment)
{
std::lock_guard g{memoryWriterMutex};
- const auto result = memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
+ const auto result =
+ memoryWriter.addSegment(properties.coreAttachmentsSegmentName, properties.providerName);
if (not result.success)
{
@@ -116,8 +123,8 @@ namespace armarx::armem::attachment
const auto providerId = armem::MemoryID(result.segmentID);
const auto entityID =
providerId
- .withEntityName(attachment.object.id.entityName) // TODO check if meaningful
- .withTimestamp(timestamp);
+ .withEntityName(attachment.object.id.entityName) // TODO check if meaningful
+ .withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -126,7 +133,7 @@ namespace armarx::armem::attachment
toAron(aronAttachment, attachment);
update.instancesData = {aronAttachment.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -143,5 +150,4 @@ namespace armarx::armem::attachment
}
-
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
index 754f4f28041429cd1bb93c9bd3b962335c984530..067d53dc8e9bbd237d9d6d8db34f98105d18f8aa 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
@@ -22,12 +22,13 @@
#pragma once
#include <mutex>
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+// #include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
#include <RobotAPI/libraries/armem_objects/types.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 399fce56d0cc22c10de24a8b1a4ab29eb6668581..131fdc655569a16b5d80488289ad2e0fa0d90983 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -14,9 +14,9 @@
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.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>
+// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
index 80d1af5590886994c04a69adb025d2767b8f7ffb..243e6a4b4c12d9fcd5b1557b4ce354f7561c2ec0 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
@@ -13,9 +13,6 @@
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.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>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index 74e29b855363472523ae328b2698059a8f9d108f..a04d95ca88d86fd722373456cccccddbd82c09a7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
@@ -18,11 +18,10 @@ armarx_add_library(
RobotAPI::ComponentPlugins
RobotAPI::Core
RobotAPI::armem_server
- RobotAPI::armem_robot
+ RobotAPI::armem_robot_state
RobotAPI::armem_objects
armem_robot_state
- armem_robot
HEADERS
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index 59a9a3474a82235d8bf19a7ee3510084c35f96f2..03988d61d5d42ecb4c039bbf93ea028f6a1d298b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -17,7 +17,7 @@
#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_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 865fa2ae35b1ce975b3be920214035674c3c9050..ebbac6dcfc78c5fbae931dcbc01fecefbe879b19 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -36,7 +36,8 @@
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -648,7 +649,7 @@ namespace armarx::armem::server::obj::instance
try
{
dto.fromAron(classInstance->data());
- robot::RobotDescription description;
+ robot_state::description::RobotDescription description;
fromAron(dto, description);
articulatedObject.description = description;
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 564bf3b460a06e036cd33b481541f4d41119cf25..0a5719891d230e2055d96bba78d947dc159af6ec 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -28,7 +28,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Names.h>
@@ -135,10 +135,10 @@ namespace armarx::armem::attachment
namespace armarx::armem::articulated_object
{
- using ArticulatedObjectDescription = armarx::armem::robot::RobotDescription;
+ using ArticulatedObjectDescription = armarx::armem::robot_state::description::RobotDescription;
- using ArticulatedObject = armarx::armem::robot::Robot;
- using ArticulatedObjects = armarx::armem::robot::Robots;
+ using ArticulatedObject = armarx::armem::robot_state::Robot;
+ using ArticulatedObjects = armarx::armem::robot_state::Robots;
} // namespace armarx::armem::articulated_object
namespace armarx::armem::marker
diff --git a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
deleted file mode 100644
index 43e6af6f4a45f3d638c182bef38e5cdc9cf50d24..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
+++ /dev/null
@@ -1,44 +0,0 @@
-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
- types.cpp
-
- 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_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
deleted file mode 100644
index 3bd7b2dfb5f03716174c6a3367fe9257f68d2526..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ /dev/null
@@ -1,106 +0,0 @@
-#include "aron_conversions.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-
-
-namespace armarx::armem::robot
-{
-
- /* 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)
- {
- aron::fromAron(dto.timestamp, bo.timestamp);
- aron::fromAron(dto.globalPose, bo.globalPose.matrix());
- aron::fromAron(dto.jointMap, bo.jointMap);
-
- }
-
- void toAron(arondto::RobotState& dto, const RobotState& bo)
- {
- aron::toAron(dto.timestamp, bo.timestamp);
- aron::toAron(dto.globalPose, bo.globalPose.matrix());
- aron::toAron(dto.jointMap, bo.jointMap);
-
- }
-
-} // namespace armarx::armem::robot
-
-
-namespace armarx::armem
-{
-
- void robot::fromAron(const arondto::ObjectClass& dto, RobotDescription& bo)
- {
- bo.name = aron::fromAron<armarx::ObjectID>(dto.id).str();
- fromAron(dto.articulatedSimoxXmlPath, bo.xml);
- }
-
- void robot::toAron(arondto::ObjectClass& dto, const RobotDescription& bo)
- {
- toAron(dto.id, ObjectID(bo.name));
- toAron(dto.articulatedSimoxXmlPath, bo.xml);
- }
-
-
- void robot::fromAron(const arondto::ObjectInstance& dto, RobotState& bo)
- {
- fromAron(dto.pose, bo);
- }
-
- void robot::toAron(arondto::ObjectInstance& dto, const RobotState& bo)
- {
- toAron(dto.pose, bo);
- }
-
-
- void robot::fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
- {
- aron::fromAron(dto.timestamp, bo.timestamp);
- aron::fromAron(dto.objectPoseGlobal, bo.globalPose);
- aron::fromAron(dto.objectJointValues, bo.jointMap);
-
- }
-
- void robot::toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
- {
- aron::toAron(dto.timestamp, bo.timestamp);
- aron::toAron(dto.objectPoseGlobal, bo.globalPose.matrix());
- aron::toAron(dto.objectJointValues, bo.jointMap);
-
- }
-
-
-} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
deleted file mode 100644
index f2ea2ba959897ad48d9aed390a569d2e8009dd17..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-
-#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);
-
- void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
- void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
-
- void fromAron(const arondto::ObjectInstance& dto, RobotState& bo);
- void toAron(arondto::ObjectInstance& dto, const RobotState& bo);
-
- void fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo);
- void toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo);
-
-} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
deleted file mode 100644
index 64d68ed6a3549fa7b0e6fa5745a1cc5731158a38..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/client/interfaces.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#pragma once
-
-
-#include "RobotAPI/libraries/armem/core/forward_declarations.h"
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
-
-namespace armarx::armem::robot
-{
- class ReaderInterface
- {
- public:
- virtual ~ReaderInterface() = default;
-
- virtual bool synchronize(Robot& obj, const armem::Time& timestamp) const = 0;
-
- virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) const = 0;
- virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) const = 0;
- };
-
- class WriterInterface
- {
- public:
- virtual ~WriterInterface() = default;
-
- // virtual bool store(const Robot& obj) = 0;
-
- virtual bool storeDescription(const RobotDescription& description,
- const armem::Time& timestamp = armem::Time::Invalid()) = 0;
-
- virtual bool storeState(const robot::RobotState& state,
- const std::string& robotTypeName,
- const std::string& robotName,
- const std::string& robotRootNodeName) = 0;
- };
-
-} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
deleted file mode 100644
index d94903c36c3df2678eddc4317751a65bc0ce506f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ /dev/null
@@ -1,91 +0,0 @@
-#pragma once
-
-#include <filesystem>
-#include <map>
-#include <vector>
-
-#include <Eigen/Geometry>
-
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/time/DateTime.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-
-namespace armarx::armem::robot
-{
- struct RobotDescription
- {
- // DateTime timestamp;
-
- std::string name;
- PackagePath xml{"", std::filesystem::path("")};
- };
-
-
- struct Twist
- {
- Eigen::Vector3f linear{Eigen::Vector3f::Zero()};
- Eigen::Vector3f angular{Eigen::Vector3f::Zero()};
- };
-
- struct PlatformState
- {
- Twist twist;
- };
-
- struct ForceTorque
- {
- Eigen::Vector3f force;
- Eigen::Vector3f gravityCompensatedForce;
- Eigen::Vector3f torque;
- Eigen::Vector3f gravityCompensatedTorque;
- };
-
- struct ToF
- {
- using DepthMatrixType = Eigen::MatrixXf; // FIXME uint16
- using DepthSigmaMatrixType = Eigen::MatrixXf; // FIXME uint8
- using TargetDetectedMatrixType = Eigen::MatrixXf; // FIXME uint8
-
- DepthMatrixType depth;
- DepthSigmaMatrixType sigma;
- TargetDetectedMatrixType targetDetected;
-
- std::string frame;
- };
-
-
- struct RobotState
- {
- using JointMap = std::map<std::string, float>;
- using Pose = Eigen::Affine3f;
-
- DateTime timestamp;
-
- Pose globalPose;
- JointMap jointMap;
-
- std::optional<armarx::armem::arondto::Proprioception> proprioception;
- };
-
-
- struct Robot
- {
- RobotDescription description;
- std::string instance;
-
- RobotState config;
-
- DateTime timestamp;
-
- std::string name() const;
- };
-
- using Robots = std::vector<Robot>;
- using RobotDescriptions = std::vector<RobotDescription>;
- using RobotStates = std::vector<RobotState>;
-
- std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs);
-
-} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index c71d1d839e5511f73cb67b13d8c80c28def503d4..21c808d635627df9a6466c988918ec1b25e9182d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -18,9 +18,12 @@ armarx_add_library(
RobotAPICore
RobotAPIInterfaces
RobotAPI::armem
- RobotAPI::armem_robot
+ RobotUnitDataStreamingReceiver
+ # RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+# RobotAPI::Core
aroncommon
-
+ # armem_objects
# System / External
Eigen3::Eigen
@@ -37,9 +40,12 @@ armarx_add_library(
client/localization/TransformReader.h
client/localization/TransformWriter.h
+ robot_conversions.h
+
aron_conversions.h
memory_ids.h
utils.h
+ types.h
SOURCES
common/localization/TransformHelper.cpp
@@ -52,9 +58,11 @@ armarx_add_library(
client/localization/TransformReader.cpp
client/localization/TransformWriter.cpp
+ robot_conversions.cpp
aron_conversions.cpp
memory_ids.cpp
utils.cpp
+ types.cpp
)
@@ -67,9 +75,58 @@ armarx_enable_aron_file_generation_for_target(
aron/Exteroception.xml
aron/TransformHeader.xml
aron/Transform.xml
+ aron/RobotDescription.xml
+ aron/RobotState.xml
+ aron/Robot.xml
)
add_library(RobotAPI::armem_robot_state ALIAS armem_robot_state)
add_subdirectory(server)
+
+
+
+# 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
+# # armem_robot_state_aron
+# HEADERS
+# types.h
+
+# aron_conversions.h
+# robot_conversions.h
+
+# SOURCES
+# types.cpp
+
+# 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_robot/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Robot.xml
similarity index 92%
rename from source/RobotAPI/libraries/armem_robot/aron/Robot.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/Robot.xml
index b7192c366e7ae685c176578b03a44d9f533f1e75..2d1a0918edf7fe628bc1654b79af3528288f9401 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Robot.xml
@@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
- <PackagePath package="RobotAPI" path="libraries/armem_robot/aron/RobotDescription.xml" />
- <PackagePath package="RobotAPI" path="libraries/armem_robot/aron/RobotState.xml" />
+ <PackagePath package="RobotAPI" path="libraries/armem_robot_state/aron/RobotDescription.xml" />
+ <PackagePath package="RobotAPI" path="libraries/armem_robot_state/aron/RobotState.xml" />
<PackagePath package="RobotAPI" path="libraries/armem/aron/MemoryID.xml" />
</AronIncludes>
@@ -27,4 +27,3 @@
</GenerateTypes>
</AronTypeDefinition>
-
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot_state/aron/RobotState.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/RobotState.xml
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 714ee296537470b7119d0ed62bb9039bb6a38553..d7bdaa27134824dc590cd6fd3787825cc21cf066 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -4,27 +4,28 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-namespace armarx::armem
+namespace armarx::armem::robot_state::localization
{
/* Transform */
void
- fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ fromAron(const arondto::Transform& dto, Transform& bo)
{
fromAron(dto.header, bo.header);
aron::fromAron(dto.transform, bo.transform);
}
void
- toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ toAron(arondto::Transform& dto, const Transform& bo)
{
toAron(dto.header, bo.header);
aron::toAron(dto.transform, bo.transform);
@@ -33,7 +34,7 @@ namespace armarx::armem
/* TransformHeader */
void
- toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ toAron(arondto::TransformHeader& dto, const TransformHeader& bo)
{
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
@@ -42,16 +43,19 @@ namespace armarx::armem
}
void
- fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ fromAron(const arondto::TransformHeader& dto, 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::robot_state::localization
+namespace armarx::armem::robot_state
+{
void
- fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ fromAron(const armarx::armem::prop::arondto::Platform& dto, robot_state::PlatformState& bo)
{
bo.twist.linear.setZero();
bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
@@ -61,13 +65,16 @@ namespace armarx::armem
}
void
- toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ toAron(armarx::armem::prop::arondto::Platform& dto, const robot_state::PlatformState& bo)
{
ARMARX_ERROR << "Not implemented yet.";
}
+} // namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::proprioception
+{
void
- fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+ fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo)
{
aron::fromAron(dto.force, bo.force);
aron::fromAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
@@ -76,26 +83,89 @@ namespace armarx::armem
}
void
- toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+ toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo)
{
aron::toAron(dto.force, bo.force);
aron::toAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
aron::toAron(dto.torque, bo.torque);
aron::toAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
}
+} // namespace armarx::armem::robot_state::proprioception
-
- void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToF& bo){
+namespace armarx::armem::robot_state::exteroception
+{
+ void
+ fromAron(const armarx::armem::exteroception::arondto::ToF& dto, ToF& bo)
+ {
bo.depth = dto.depth;
bo.sigma = dto.sigma;
bo.targetDetected = dto.targetDetected;
}
- void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToF& bo){
+ void
+ toAron(armarx::armem::exteroception::arondto::ToF& dto, const ToF& bo)
+ {
dto.depth = bo.depth;
dto.sigma = bo.sigma;
dto.targetDetected = bo.targetDetected;
}
+} // namespace armarx::armem::robot_state::exteroception
+
+namespace armarx::armem::robot_state::description
+{
+ 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);
+ }
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+
+ /* 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 */
+
+ /* RobotState */
+
+ void
+ fromAron(const arondto::RobotState& dto, RobotState& bo)
+ {
+ aron::fromAron(dto.timestamp, bo.timestamp);
+ aron::fromAron(dto.globalPose, bo.globalPose.matrix());
+ aron::fromAron(dto.jointMap, bo.jointMap);
+ }
+
+ void
+ toAron(arondto::RobotState& dto, const RobotState& bo)
+ {
+ aron::toAron(dto.timestamp, bo.timestamp);
+ aron::toAron(dto.globalPose, bo.globalPose.matrix());
+ aron::toAron(dto.jointMap, bo.jointMap);
+ }
-} // namespace armarx::armem
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 3df14cb39abfd1231b8222fff7e3d7b3a15905a3..d0a5716c732beacf0bde1dc033778fcee54ceb7f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -21,15 +21,28 @@
#pragma once
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+
+// #include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+// #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+// #include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
namespace armarx::armem
{
- namespace robot_state
+ namespace robot_state::localization
{
struct Transform;
struct TransformHeader;
+ } // namespace robot_state::localization
+
+ namespace robot_state
+ {
struct JointState;
} // namespace robot_state
@@ -38,7 +51,7 @@ namespace armarx::armem
{
struct Platform;
struct ForceTorque;
- }
+ } // namespace prop::arondto
namespace exteroception::arondto
{
@@ -54,21 +67,60 @@ namespace armarx::armem
} // namespace arondto
+} // namespace armarx::armem
+
+namespace armarx::armem::robot_state::localization
+{
+
+ void fromAron(const arondto::Transform& dto, Transform& bo);
+ void toAron(arondto::Transform& dto, const Transform& bo);
+
+ void fromAron(const arondto::TransformHeader& dto, TransformHeader& bo);
+ void toAron(arondto::TransformHeader& dto, const TransformHeader& bo);
+} // namespace armarx::armem::robot_state::localization
- void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
- void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
+namespace armarx::armem::robot_state
+{
- void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
- void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
- void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
- void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+} // namespace armarx::armem::robot_state
- void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
- void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+namespace armarx::armem::robot_state::proprioception
+{
+
+ void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo);
+ void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo);
+
+} // namespace armarx::armem::robot_state::proprioception
+
+namespace armarx::armem::robot_state::exteroception
+{
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, ToF& bo);
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const ToF& bo);
+
+
+} // namespace armarx::armem::robot_state::exteroception
+
+namespace armarx::armem::robot_state::description
+{
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
+
+} // namespace armarx::armem::robot_state::description
+
+namespace armarx::armem::robot_state
+{
+ // 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 armarx::armem::exteroception::arondto::ToF& dto, robot::ToF& bo);
- void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToF& bo);
+ void fromAron(const arondto::Robot& dto, Robot& bo);
+ void toAron(arondto::Robot& dto, const Robot& bo);
+ void fromAron(const arondto::RobotState& dto, RobotState& bo);
+ void toAron(arondto::RobotState& dto, const RobotState& bo);
-} // namespace armarx::armem
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h b/source/RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8d8ae3834fee00c46f2f9215db6163d6b7fe795
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h
@@ -0,0 +1,24 @@
+#pragma once
+
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::robot_state::client::common
+{
+ class ReaderInterface
+ {
+ public:
+ virtual ~ReaderInterface() = default;
+
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) const = 0;
+
+ virtual Robot get(const description::RobotDescription& description,
+ const armem::Time& timestamp) const = 0;
+ virtual std::optional<Robot> get(const std::string& name,
+ const armem::Time& timestamp) const = 0;
+ };
+
+
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 8a3b67e984d51a40d85e44113bc54a872233d910..9827cf6b7e9dc65921d173d446a60e2a5e200b75 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -18,18 +18,17 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.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>
-#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace fs = ::std::filesystem;
-namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::client::common
{
RobotReader::RobotReader(const RobotReader& r) :
properties(r.properties),
@@ -64,7 +63,7 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::Robot>
+ std::optional<Robot>
RobotReader::get(const std::string& name, const armem::Time& timestamp) const
{
const auto description = queryDescription(name, timestamp);
@@ -78,10 +77,11 @@ namespace armarx::armem::robot_state
return get(*description, timestamp);
}
- robot::Robot
- RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ Robot
+ RobotReader::get(const description::RobotDescription& description,
+ const armem::Time& timestamp) const
{
- robot::Robot robot{.description = description,
+ Robot robot{.description = description,
.instance = "", // TODO(fabian.reister):
.config = {}, // will be populated by synchronize
.timestamp = timestamp};
@@ -105,7 +105,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp) const
+ RobotReader::synchronize(Robot& obj, const armem::Time& timestamp) const
{
const auto tsStartFunctionInvokation = armem::Time::Now();
@@ -134,7 +134,7 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotDescription>
+ std::optional<description::RobotDescription>
RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp) const
{
@@ -182,11 +182,11 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- std::optional<robot::RobotState>
- RobotReader::queryState(const robot::RobotDescription& description,
+ std::optional<RobotState>
+ RobotReader::queryState(const description::RobotDescription& description,
const armem::Time& timestamp) const
{
- std::optional<robot::RobotState> robotState = queryJointState(description, timestamp);
+ std::optional<RobotState> robotState = queryJointState(description, timestamp);
if (robotState)
{
@@ -202,8 +202,8 @@ namespace armarx::armem::robot_state
return robotState;
}
- std::optional<robot::RobotState>
- RobotReader::queryJointState(const robot::RobotDescription& description,
+ std::optional<RobotState>
+ RobotReader::queryJointState(const description::RobotDescription& description,
const armem::Time& timestamp) const
{
const auto proprioception = queryProprioception(description, timestamp);
@@ -216,18 +216,18 @@ namespace armarx::armem::robot_state
}
const auto jointMap = proprioception->joints.position;
- return robot::RobotState{.timestamp = timestamp,
- .globalPose = robot::RobotState::Pose::Identity(),
+ return RobotState{.timestamp = timestamp,
+ .globalPose = RobotState::Pose::Identity(),
.jointMap = jointMap,
.proprioception = proprioception};
}
- std::optional<::armarx::armem::robot_state::Transform>
- RobotReader::queryOdometryPose(const robot::RobotDescription& description,
+ std::optional<::armarx::armem::robot_state::localization::Transform>
+ RobotReader::queryOdometryPose(const description::RobotDescription& description,
const armem::Time& timestamp) const
{
- common::robot_state::localization::TransformQuery query{
+ robot_state::localization::TransformQuery query{
.header = {.parentFrame = OdometryFrame,
.frame = constants::robotRootNodeName,
.agent = description.name,
@@ -250,7 +250,7 @@ namespace armarx::armem::robot_state
}
std::optional<armarx::armem::arondto::Proprioception>
- RobotReader::queryProprioception(const robot::RobotDescription& description,
+ RobotReader::queryProprioception(const description::RobotDescription& description,
const armem::Time& timestamp) const // Why timestamp?!?!
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -292,7 +292,7 @@ namespace armarx::armem::robot_state
}
RobotReader::JointTrajectory
- RobotReader::queryJointStates(const robot::RobotDescription& description,
+ RobotReader::queryJointStates(const description::RobotDescription& description,
const armem::Time& begin,
const armem::Time& end) const
{
@@ -332,8 +332,8 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::PlatformState>
- RobotReader::queryPlatformState(const robot::RobotDescription& description,
+ std::optional<PlatformState>
+ RobotReader::queryPlatformState(const description::RobotDescription& description,
const armem::Time& timestamp) const
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -374,8 +374,8 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotState::Pose>
- RobotReader::queryGlobalPose(const robot::RobotDescription& description,
+ std::optional<RobotState::Pose>
+ RobotReader::queryGlobalPose(const description::RobotDescription& description,
const armem::Time& timestamp) const
{
try
@@ -397,7 +397,7 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotState>
+ std::optional<RobotState>
RobotReader::getRobotState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
@@ -431,7 +431,7 @@ namespace armarx::armem::robot_state
}
// Here, we access the RobotUnit streaming data stored in the proprioception segment.
- return robot::convertRobotState(*instance);
+ return convertRobotState(*instance);
}
// FIXME remove this, use armem/util/util.h
@@ -520,8 +520,8 @@ namespace armarx::armem::robot_state
}
// force torque for left and right
- std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
- RobotReader::queryForceTorque(const robot::RobotDescription& description,
+ std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
+ RobotReader::queryForceTorque(const description::RobotDescription& description,
const armem::Time& timestamp) const
{
@@ -562,8 +562,8 @@ namespace armarx::armem::robot_state
}
// force torque for left and right
- std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
- RobotReader::queryForceTorques(const robot::RobotDescription& description,
+ std::optional<std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
+ RobotReader::queryForceTorques(const description::RobotDescription& description,
const armem::Time& start,
const armem::Time& end) const
{
@@ -604,8 +604,8 @@ namespace armarx::armem::robot_state
}
}
- std::optional<std::map<RobotReader::Hand, robot::ToF>>
- RobotReader::queryToF(const robot::RobotDescription& description,
+ std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
+ RobotReader::queryToF(const description::RobotDescription& description,
const armem::Time& timestamp) const
{
// Query all entities from provider.
@@ -644,11 +644,11 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::PlatformState>
+ std::optional<PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
- std::optional<robot::PlatformState> platformState;
+ std::optional<PlatformState> platformState;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -669,7 +669,7 @@ namespace armarx::armem::robot_state
tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
ARMARX_CHECK(proprioception.has_value());
- platformState = robot::PlatformState(); // initialize optional
+ platformState = PlatformState(); // initialize optional
fromAron(proprioception->platform, platformState.value());
});
@@ -692,11 +692,11 @@ namespace armarx::armem::robot_state
throw LocalException("Unknown hand name `" + name + "`!");
}
- std::map<RobotReader::Hand, robot::ForceTorque>
+ std::map<RobotReader::Hand, proprioception::ForceTorque>
RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
- std::map<RobotReader::Hand, robot::ForceTorque> forceTorques;
+ std::map<RobotReader::Hand, proprioception::ForceTorque> forceTorques;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -719,7 +719,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
- robot::ForceTorque forceTorque;
+ proprioception::ForceTorque forceTorque;
fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
@@ -730,11 +730,11 @@ namespace armarx::armem::robot_state
return forceTorques;
}
- std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
- std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> forceTorques;
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>> forceTorques;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -751,7 +751,7 @@ namespace armarx::armem::robot_state
for (const auto& [handName, dtoFt] : proprioception->forceTorque)
{
- robot::ForceTorque forceTorque;
+ proprioception::ForceTorque forceTorque;
fromAron(dtoFt, forceTorque);
const auto hand = fromHandName(handName);
@@ -762,10 +762,10 @@ namespace armarx::armem::robot_state
return forceTorques;
}
- std::map<RobotReader::Hand, robot::ToF>
+ std::map<RobotReader::Hand, exteroception::ToF>
RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
{
- std::map<RobotReader::Hand, robot::ToF> tofs;
+ std::map<RobotReader::Hand, exteroception::ToF> tofs;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -792,7 +792,7 @@ namespace armarx::armem::robot_state
{
ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
- robot::ToF tof;
+ exteroception::ToF tof;
fromAron(dtoFt, tof);
const auto hand = fromHandName(handName);
@@ -803,9 +803,9 @@ namespace armarx::armem::robot_state
return tofs;
}
- std::optional<robot::RobotDescription>
+ std::optional<description::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ const std::string& name) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -822,22 +822,22 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::convertRobotDescription(*instance);
+ return convertRobotDescription(*instance);
}
- std::vector<robot::RobotDescription>
+ std::vector<description::RobotDescription>
RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(constants::descriptionCoreSegment);
- std::vector<robot::RobotDescription> descriptions;
+ std::vector<description::RobotDescription> descriptions;
coreSegment.forEachInstance(
[&descriptions](const wm::EntityInstance& instance)
{
- if (const std::optional<robot::RobotDescription> desc =
- robot::convertRobotDescription(instance))
+ if (const std::optional<description::RobotDescription> desc =
+ convertRobotDescription(instance))
{
descriptions.push_back(desc.value());
}
@@ -846,7 +846,7 @@ namespace armarx::armem::robot_state
return descriptions;
}
- std::vector<robot::RobotDescription>
+ std::vector<description::RobotDescription>
RobotReader::queryDescriptions(const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -892,4 +892,4 @@ namespace armarx::armem::robot_state
return {};
}
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 650227dee1666bb124d4941a13c16088f62d88a5..833783371a921d76266ed1e14b30310f581c38d5 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -28,11 +28,11 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::client::common
{
/**
* @brief The RobotReader class.
@@ -40,7 +40,7 @@ namespace armarx::armem::robot_state
* The purpose of this class is to synchronize the armem data structure armem::Robot
* with the memory.
*/
- class RobotReader : virtual public robot::ReaderInterface
+ class RobotReader : virtual public ReaderInterface
{
public:
RobotReader() = default;
@@ -51,41 +51,41 @@ namespace armarx::armem::robot_state
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- [[nodiscard]] bool synchronize(robot::Robot& obj,
- const armem::Time& timestamp) const override;
+ [[nodiscard]] bool synchronize(Robot& obj, const armem::Time& timestamp) const override;
- std::optional<robot::Robot> get(const std::string& name,
- const armem::Time& timestamp) const override;
- robot::Robot get(const robot::RobotDescription& description,
- const armem::Time& timestamp) const override;
+ std::optional<Robot> get(const std::string& name,
+ const armem::Time& timestamp) const override;
+ Robot get(const description::RobotDescription& description,
+ const armem::Time& timestamp) const override;
- std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp) const;
+ std::optional<description::RobotDescription>
+ queryDescription(const std::string& name, const armem::Time& timestamp) const;
- std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp) const;
+ std::vector<description::RobotDescription>
+ queryDescriptions(const armem::Time& timestamp) const;
- std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<RobotState> queryState(const description::RobotDescription& description,
+ const armem::Time& timestamp) const;
- std::optional<robot::RobotState> queryJointState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<RobotState> queryJointState(const description::RobotDescription& description,
+ const armem::Time& timestamp) const;
std::optional<armarx::armem::arondto::Proprioception>
- queryProprioception(const robot::RobotDescription& description,
+ queryProprioception(const description::RobotDescription& description,
const armem::Time& timestamp) const;
- using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
+ using JointTrajectory = std::map<armem::Time, RobotState::JointMap>;
- JointTrajectory queryJointStates(const robot::RobotDescription& description,
+ JointTrajectory queryJointStates(const description::RobotDescription& description,
const armem::Time& begin,
const armem::Time& end) const;
- std::optional<robot::RobotState::Pose>
- queryGlobalPose(const robot::RobotDescription& description,
+ std::optional<RobotState::Pose>
+ queryGlobalPose(const description::RobotDescription& description,
const armem::Time& timestamp) const;
- std::optional<robot::PlatformState>
- queryPlatformState(const robot::RobotDescription& description,
+ std::optional<PlatformState>
+ queryPlatformState(const description::RobotDescription& description,
const armem::Time& timestamp) const;
void setSyncTimeout(const armem::Duration& duration);
@@ -97,18 +97,20 @@ namespace armarx::armem::robot_state
Right
};
- std::optional<std::map<Hand, robot::ForceTorque>>
- queryForceTorque(const robot::RobotDescription& description,
+ std::optional<std::map<Hand, proprioception::ForceTorque>>
+ queryForceTorque(const description::RobotDescription& description,
const armem::Time& timestamp) const;
- std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
- queryForceTorques(const robot::RobotDescription& description,
+ std::optional<
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
+ queryForceTorques(const description::RobotDescription& description,
const armem::Time& start,
const armem::Time& end) const;
- std::optional<std::map<Hand, robot::ToF>>
- queryToF(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+ std::optional<std::map<Hand, exteroception::ToF>>
+ queryToF(const description::RobotDescription& description,
+ const armem::Time& timestamp) const;
/**
* @brief retrieve the robot's pose in the odometry frame.
@@ -117,10 +119,10 @@ namespace armarx::armem::robot_state
*
* @param description
* @param timestamp
- * @return std::optional<robot::RobotState::Pose>
+ * @return std::optional<RobotState::Pose>
*/
- std::optional<::armarx::armem::robot_state::Transform>
- queryOdometryPose(const robot::RobotDescription& description,
+ std::optional<::armarx::armem::robot_state::localization::Transform>
+ queryOdometryPose(const description::RobotDescription& description,
const armem::Time& timestamp) const;
protected:
@@ -129,13 +131,13 @@ namespace armarx::armem::robot_state
armem::Duration sleepAfterFailure = armem::Duration::MicroSeconds(0);
private:
- std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ std::optional<RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
- std::optional<robot::RobotDescription>
+ std::optional<description::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::vector<robot::RobotDescription>
+ std::vector<description::RobotDescription>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
std::optional<armarx::armem::arondto::Proprioception>
@@ -145,19 +147,18 @@ namespace armarx::armem::robot_state
JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
- std::optional<robot::PlatformState>
- getRobotPlatformState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ std::optional<PlatformState> getRobotPlatformState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
- std::map<RobotReader::Hand, robot::ForceTorque>
+ std::map<RobotReader::Hand, proprioception::ForceTorque>
getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::map<RobotReader::Hand, robot::ToF> getToF(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ std::map<RobotReader::Hand, exteroception::ToF>
+ getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const;
struct Properties
{
@@ -168,7 +169,13 @@ namespace armarx::armem::robot_state
armem::client::Reader memoryReader;
mutable std::mutex memoryReaderMutex;
- client::robot_state::localization::TransformReader transformReader;
+ localization::TransformReader transformReader;
};
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
+
+
+namespace armarx::armem::robot_state
+{
+ using client::common::RobotReader;
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
index e92e13d6421c71313bdd04a488e1f43397d1f222..5d6f6a0f88f420b578ad53f3753dfec414d413de 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -19,20 +19,19 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/TransformHeader.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace fs = ::std::filesystem;
-namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::client::common
{
RobotWriter::RobotWriter(const RobotWriter& r) :
properties(r.properties), propertyPrefix(r.propertyPrefix), memoryWriter(r.memoryWriter)
@@ -65,7 +64,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotWriter::storeDescription(const robot::RobotDescription& description,
+ RobotWriter::storeDescription(const description::RobotDescription& description,
const armem::Time& timestamp)
{
const auto validTimestamp = timestamp.isInvalid() ? armarx::Clock::Now() : timestamp;
@@ -177,7 +176,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotWriter::storeState(const robot::RobotState& state,
+ RobotWriter::storeState(const RobotState& state,
const std::string& robotTypeName,
const std::string& robotName,
const std::string& robotRootNodeName)
@@ -191,4 +190,4 @@ namespace armarx::armem::robot_state
}
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
index d8ca351d6cf8643960f1707a49ef26b9697d4082..511845de0359b9e618662727783c03bd91d8e890 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
@@ -28,11 +28,11 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::client::common
{
/**
* @brief The RobotReader class.
@@ -40,7 +40,7 @@ namespace armarx::armem::robot_state
* The purpose of this class is to synchronize the armem data structure armem::Robot
* with the memory.
*/
- class RobotWriter : virtual public robot::WriterInterface
+ class RobotWriter : virtual public WriterInterface
{
public:
RobotWriter() = default;
@@ -52,11 +52,11 @@ namespace armarx::armem::robot_state
[[nodiscard]] bool
- storeDescription(const robot::RobotDescription& description,
+ storeDescription(const description::RobotDescription& description,
const armem::Time& timestamp = armem::Time::Invalid()) override;
- [[nodiscard]] bool storeState(const robot::RobotState& state,
+ [[nodiscard]] bool storeState(const RobotState& state,
const std::string& robotTypeName,
const std::string& robotName,
const std::string& robotRootNodeName) override;
@@ -83,4 +83,10 @@ namespace armarx::armem::robot_state
armem::client::Writer memoryWriter;
};
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
+
+
+namespace armarx::armem::robot_state
+{
+ using client::common::RobotWriter;
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index e23178dfa4a884654b8ba924a71006dbd88e8e8b..e7a7838a65d5b37b6b3f78f930744fa7311920c6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -13,7 +13,7 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Clock.h>
-namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::client::common
{
// TODO(fabian.reister): register property defs
void
@@ -29,8 +29,8 @@ namespace armarx::armem::robot_state
// const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
- const robot::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{"", ""}};
+ const description::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{"", ""}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
@@ -54,8 +54,8 @@ namespace armarx::armem::robot_state
// const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
- const robot::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{"", ""}};
+ const description::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{"", ""}};
const auto robotState = queryJointState(robotDescription, timestamp);
if (not robotState)
@@ -145,7 +145,7 @@ namespace armarx::armem::robot_state
return nullptr;
}
- std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
@@ -157,4 +157,4 @@ namespace armarx::armem::robot_state
}
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index d0a672c21ba5ae953d3bc3b8bed2333a9f6efa97..e0142d356727be6de749edabf34e49e0f75c2c22 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -27,11 +27,11 @@
#include "RobotReader.h"
-namespace armarx::armem::robot_state
+namespace armarx::armem::robot_state::client::common
{
/**
* @brief The VirtualRobotReader class.
- *
+ *robot
* The aim of this class is to obtain a virtual robot instance and synchronize it
* with the data (joint positions, global pose, ...) stored in the working memory.
*
@@ -71,7 +71,7 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
- std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
queryForceTorque(const std::string& name, const armem::Time& timestamp);
using RobotReader::queryForceTorque;
@@ -84,4 +84,9 @@ namespace armarx::armem::robot_state
bool blocking = true);
};
-} // namespace armarx::armem::robot_state
+} // namespace armarx::armem::robot_state::client::common
+
+namespace armarx::armem::robot_state
+{
+ using client::common::VirtualRobotReader;
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index e7525729d3f6d59d4bb8c3feaf237f5ff34d18de..48dc75da8b590436b65c5799591a5f25ad18be69 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -14,7 +14,7 @@
#include <ArmarXCore/core/time/Clock.h>
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/armem_robot_state/types.h"
#include "constants.h"
@@ -50,7 +50,7 @@ namespace armarx::armem::robot_state
{
const auto filename = robot.getFilename();
- const robot::RobotDescription desc{.name = robot.getType(),
+ const description::RobotDescription desc{.name = robot.getType(),
.xml = resolvePackagePath(filename)};
return RobotWriter::storeDescription(desc, timestamp);
@@ -59,9 +59,9 @@ namespace armarx::armem::robot_state
bool
VirtualRobotWriter::storeState(const VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
- const robot::RobotState robotState{.timestamp = timestamp,
+ const RobotState robotState{.timestamp = timestamp,
.globalPose =
- robot::RobotState::Pose(robot.getGlobalPose()),
+ RobotState::Pose(robot.getGlobalPose()),
.jointMap = robot.getJointValues(),
.proprioception = std::nullopt};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h b/source/RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..d4974452245a9156508d907acafda474418d6d49
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/WriterInterface.h
@@ -0,0 +1,27 @@
+#pragma once
+
+
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::robot_state::client::common
+{
+
+ class WriterInterface
+ {
+ public:
+ virtual ~WriterInterface() = default;
+
+ // virtual bool store(const Robot& obj) = 0;
+
+ virtual bool storeDescription(const description::RobotDescription& description,
+ const armem::Time& timestamp = armem::Time::Invalid()) = 0;
+
+ virtual bool storeState(const RobotState& state,
+ const std::string& robotTypeName,
+ const std::string& robotName,
+ const std::string& robotRootNodeName) = 0;
+ };
+
+} // namespace armarx::armem::robot_state::client::common
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 5a221a342be4e0803c1cb28e708352b10b7e98f6..8613833baf8a001359f4878a5ff69a7d993658ce 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -57,7 +57,7 @@
#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
TransformReader::TransformReader(const TransformReader& t) :
memoryReader(t.memoryReader), properties(t.properties), propertyPrefix(t.propertyPrefix)
@@ -173,7 +173,7 @@ namespace armarx::armem::client::robot_state::localization
const auto& localizationCoreSegment =
qResult.memory.getCoreSegment(properties.localizationSegment);
- using armarx::armem::common::robot_state::localization::TransformHelper;
+ using armarx::armem::robot_state::localization::TransformHelper;
return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
catch (...)
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 32b3c2fc12eb46c9c67baa362d92f19552ada51d..37dc53c5c190e576af8fcc13d2d12772b719d921 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -27,7 +27,7 @@
#include "interfaces.h"
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
/**
* @defgroup Component-ExampleClient ExampleClient
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index f01f8e9241a5ebc605c6b158f3256407a5e9bd17..1a718cdc33df6ddaf9ce95f8abeef495465db48a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -47,7 +47,7 @@
#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
TransformWriter::~TransformWriter() = default;
@@ -82,7 +82,8 @@ namespace armarx::armem::client::robot_state::localization
}
bool
- TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
+ TransformWriter::commitTransform(
+ const ::armarx::armem::robot_state::localization::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
@@ -114,4 +115,4 @@ namespace armarx::armem::client::robot_state::localization
return updateResult.success;
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 182981846d14eb775256b3a29aeed9e2a4eb483b..9daf8f76360201f5ca0a65271ab1dbc17f0d42df 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -29,7 +29,7 @@
#include "interfaces.h"
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
/**
@@ -56,7 +56,8 @@ namespace armarx::armem::client::robot_state::localization
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
- bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
+ bool commitTransform(
+ const ::armarx::armem::robot_state::localization::Transform& transform) override;
private:
@@ -72,4 +73,4 @@ namespace armarx::armem::client::robot_state::localization
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::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
index 5689058cc27f39dbe86b0e5e1b3171466407ac50..b78e8693ca42e64c40e299fe921e10808915a7ab 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -30,11 +30,11 @@
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::client::robot_state::localization
+namespace armarx::armem::robot_state::client::localization
{
- using armarx::armem::common::robot_state::localization::TransformQuery;
- using armarx::armem::common::robot_state::localization::TransformResult;
+ using armarx::armem::robot_state::localization::TransformQuery;
+ using armarx::armem::robot_state::localization::TransformResult;
class TransformInterface
{
@@ -63,7 +63,8 @@ namespace armarx::armem::client::robot_state::localization
public:
~TransformWriterInterface() override = default;
- virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
+ virtual bool
+ commitTransform(const ::armarx::armem::robot_state::localization::Transform& transform) = 0;
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index 252dd7869ec119acfb5c8bcc38a7bc0a5e3d9a6f..ac13c1fa3180d3bfe22edce4e5e8c0a54d7585db 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -24,7 +24,7 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-namespace armarx::armem::common::robot_state::localization
+namespace armarx::armem::robot_state::localization
{
template <class... Args>
@@ -44,7 +44,7 @@ namespace armarx::armem::common::robot_state::localization
query.header.agent + "`."};
}
- const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
+ const std::vector<Eigen::Isometry3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
const std::optional<armem::Time> sanitizedTimestamp =
@@ -76,8 +76,8 @@ namespace armarx::armem::common::robot_state::localization
"'. No memory data in time range. Reference time " + sanitizedTimestamp.value().toTimeString()};
}
- const Eigen::Affine3f transform = std::accumulate(
- transforms.begin(), transforms.end(), Eigen::Affine3f::Identity(), std::multiplies<>());
+ const Eigen::Isometry3f transform = std::accumulate(
+ transforms.begin(), transforms.end(), Eigen::Isometry3f::Identity(), std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
@@ -97,14 +97,14 @@ namespace armarx::armem::common::robot_state::localization
{
ARMARX_DEBUG << "TF chain is empty";
return {.header = query.header,
- .transforms = std::vector<Eigen::Affine3f>{},
+ .transforms = std::vector<Eigen::Isometry3f>{},
.status = TransformChainResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
" -> " + query.header.frame + "' for robot `" +
query.header.agent + "`."};
}
- const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
+ const std::vector<Eigen::Isometry3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
if (transforms.empty())
{
@@ -227,7 +227,7 @@ namespace armarx::armem::common::robot_state::localization
}
template <class... Args>
- std::vector<Eigen::Affine3f>
+ std::vector<Eigen::Isometry3f>
TransformHelper::_obtainTransforms(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const std::vector<std::string>& tfChain,
@@ -241,7 +241,7 @@ namespace armarx::armem::common::robot_state::localization
try
{
- std::vector<Eigen::Affine3f> transforms;
+ std::vector<Eigen::Isometry3f> transforms;
transforms.reserve(tfChain.size());
std::transform(tfChain.begin(),
tfChain.end(),
@@ -272,7 +272,7 @@ namespace armarx::armem::common::robot_state::localization
}
template <class... Args>
- Eigen::Affine3f
+ Eigen::Isometry3f
TransformHelper::_obtainTransform(
const std::string& entityName,
const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
@@ -287,10 +287,10 @@ namespace armarx::armem::common::robot_state::localization
// {
// // TODO(fabian.reister): fixme boom
// ARMARX_ERROR << "No snapshots received.";
- // return Eigen::Affine3f::Identity();
+ // return Eigen::Isometry3f::Identity();
// }
- std::vector<::armarx::armem::robot_state::Transform> transforms;
+ std::vector<::armarx::armem::robot_state::localization::Transform> transforms;
auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
ARMARX_CHECK(snapshot) << "No snapshot found before or at time " << timestamp;
@@ -321,20 +321,20 @@ namespace armarx::armem::common::robot_state::localization
return transforms.front().transform;
}
- ::armarx::armem::robot_state::Transform
+ ::armarx::armem::robot_state::localization::Transform
TransformHelper::_convertEntityToTransform(const armem::wm::EntityInstance& item)
{
arondto::Transform aronTransform;
aronTransform.fromAron(item.data());
- ::armarx::armem::robot_state::Transform transform;
+ ::armarx::armem::robot_state::localization::Transform transform;
fromAron(aronTransform, transform);
return transform;
}
auto
- findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
+ findFirstElementAfter(const std::vector<::armarx::armem::robot_state::localization::Transform>& transforms,
const armem::Time& timestamp)
{
const auto comp = [](const armem::Time& timestamp, const auto& transform)
@@ -342,7 +342,7 @@ namespace armarx::armem::common::robot_state::localization
const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
- auto timestampBeyond = [timestamp](const ::armarx::armem::robot_state::Transform& transform)
+ auto timestampBeyond = [timestamp](const localization::Transform& transform)
{ return transform.header.timestamp > timestamp; };
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
@@ -352,9 +352,9 @@ namespace armarx::armem::common::robot_state::localization
return poseNextIt;
}
- Eigen::Affine3f
+ Eigen::Isometry3f
TransformHelper::_interpolateTransform(
- const std::vector<::armarx::armem::robot_state::Transform>& queue,
+ const std::vector<::armarx::armem::robot_state::localization::Transform>& queue,
armem::Time timestamp)
{
ARMARX_TRACE;
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 80cdea39921497a5c2c369416d59115d7dffc4c5..fb1ffbc25eeee21b6e4bcd1fd368714db3ad6e85 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -21,8 +21,8 @@
#pragma once
-#include <vector>
#include <optional>
+#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -30,103 +30,75 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
-
-namespace armarx::armem::common::robot_state::localization
+namespace armarx::armem::robot_state::localization
{
- using armarx::armem::common::robot_state::localization::TransformQuery;
- using armarx::armem::common::robot_state::localization::TransformResult;
-
-
+ using armarx::armem::robot_state::localization::TransformQuery;
+ using armarx::armem::robot_state::localization::TransformResult;
class TransformHelper
{
public:
+ static TransformResult
+ lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
- static
- TransformResult
- lookupTransform(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
-
- static
- TransformResult
- lookupTransform(
- const armem::server::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
+ static TransformResult
+ lookupTransform(const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
- static
- TransformChainResult
- lookupTransformChain(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
+ static TransformChainResult
+ lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
- static
- TransformChainResult
- lookupTransformChain(
- const armem::server::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query);
+ static TransformChainResult
+ lookupTransformChain(const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
private:
+ template <class... Args>
+ static TransformResult
+ _lookupTransform(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
- template <class ...Args>
- static
- TransformResult
- _lookupTransform(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const TransformQuery& query);
-
-
- template <class ...Args>
- static
- TransformChainResult
- _lookupTransformChain(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const TransformQuery& query);
-
-
- template <class ...Args>
- static
- std::vector<std::string>
- _buildTransformChain(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const TransformQuery& query);
-
-
- template <class ...Args>
- static
- std::vector<Eigen::Affine3f>
- _obtainTransforms(
- const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
- const std::vector<std::string>& tfChain,
- const std::string& agent,
- const armem::Time& timestamp);
-
-
- template <class ...Args>
- static
- Eigen::Affine3f
- _obtainTransform(
- const std::string& entityName,
- const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
- const armem::Time& timestamp);
-
- template <class ...Args>
- static
- std::optional<armarx::core::time::DateTime>
- _obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp);
-
- static
- Eigen::Affine3f
- _interpolateTransform(
- const std::vector<::armarx::armem::robot_state::Transform>& queue,
- armem::Time timestamp);
- static
- ::armarx::armem::robot_state::Transform
- _convertEntityToTransform(
- const armem::wm::EntityInstance& item);
+ template <class... Args>
+ static TransformChainResult
+ _lookupTransformChain(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ template <class... Args>
+ static std::vector<std::string>
+ _buildTransformChain(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const TransformQuery& query);
+
+
+ template <class... Args>
+ static std::vector<Eigen::Isometry3f>
+ _obtainTransforms(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp);
+
+
+ template <class... Args>
+ static Eigen::Isometry3f
+ _obtainTransform(const std::string& entityName,
+ const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
+ const armem::Time& timestamp);
+
+ template <class... Args>
+ static std::optional<armarx::core::time::DateTime>
+ _obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const armem::Time& timestamp);
+
+ static Eigen::Isometry3f _interpolateTransform(
+ const std::vector<::armarx::armem::robot_state::localization::Transform>& queue,
+ armem::Time timestamp);
+ static ::armarx::armem::robot_state::localization::Transform
+ _convertEntityToTransform(const armem::wm::EntityInstance& item);
};
-} // namespace armarx::armem::common::robot_state::localization
+} // namespace armarx::armem::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index 26311961772c03b70519de0cde245b44e17bf0db..8bd428ce382fea070e611bde7893be991910b5db 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -26,14 +26,13 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::common::robot_state::localization
+namespace armarx::armem::robot_state::localization
{
struct TransformResult
{
- ::armarx::armem::robot_state::Transform transform;
+ ::armarx::armem::robot_state::localization::Transform transform;
enum class Status
{
@@ -53,8 +52,8 @@ namespace armarx::armem::common::robot_state::localization
struct TransformChainResult
{
- ::armarx::armem::robot_state::TransformHeader header;
- std::vector<Eigen::Affine3f> transforms;
+ ::armarx::armem::robot_state::localization::TransformHeader header;
+ std::vector<Eigen::Isometry3f> transforms;
enum class Status
{
@@ -74,9 +73,9 @@ namespace armarx::armem::common::robot_state::localization
struct TransformQuery
{
- ::armarx::armem::robot_state::TransformHeader header;
+ ::armarx::armem::robot_state::localization::TransformHeader header;
// bool exact;
};
-} // namespace armarx::armem::common::robot_state::localization
+} // namespace armarx::armem::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
index 2f219eff39376ed3ecc44e47b0780fd1328785a0..07cdcd42a54502061b5c23326a81291c53dab7b0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
@@ -22,16 +22,15 @@
#include "memory_ids.h"
-
-namespace armarx::armem
+namespace armarx::armem::robot_state // ::constants
{
- const MemoryID robot_state::memoryID { "RobotState" };
+ const MemoryID memoryID{"RobotState"};
- const MemoryID robot_state::descriptionSegmentID { memoryID.withCoreSegmentName("Description") };
- const MemoryID robot_state::proprioceptionSegmentID { memoryID.withCoreSegmentName("Proprioception") };
- const MemoryID robot_state::exteroceptionSegmentID { memoryID.withCoreSegmentName("Exteroception") };
- const MemoryID robot_state::localizationSegmentID { memoryID.withCoreSegmentName("Localization") };
+ const MemoryID descriptionSegmentID{memoryID.withCoreSegmentName("Description")};
+ const MemoryID proprioceptionSegmentID{memoryID.withCoreSegmentName("Proprioception")};
+ const MemoryID exteroceptionSegmentID{memoryID.withCoreSegmentName("Exteroception")};
+ const MemoryID localizationSegmentID{memoryID.withCoreSegmentName("Localization")};
-}
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
index f653236b1f62ec8aef905d45a7ee86af079b05b9..e72fd8f7acef6c7b9418d57d63326df5a2bb35b0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
@@ -24,7 +24,6 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::robot_state
{
@@ -35,4 +34,4 @@ namespace armarx::armem::robot_state
extern const MemoryID exteroceptionSegmentID;
extern const MemoryID localizationSegmentID;
-} // namespace armarx::armem::objects
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
similarity index 71%
rename from source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
rename to source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
index 8b7829a2f0c3f6dadd54bf2ce624c9fcbe793efd..22790b52cc21f8af15877f147d620e735b79f520 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
@@ -2,23 +2,22 @@
#include <filesystem>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace fs = ::std::filesystem;
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state
{
- std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance)
+ std::optional<description::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance)
{
arondto::RobotDescription aronRobotDescription;
try
@@ -31,8 +30,7 @@ namespace armarx::armem::robot
return std::nullopt;
}
- RobotDescription robotDescription
- {
+ description::RobotDescription robotDescription{
.name = "",
.xml = ::armarx::PackagePath("", fs::path("")) // initialize empty, no default c'tor
};
@@ -42,8 +40,8 @@ namespace armarx::armem::robot
return robotDescription;
}
-
- std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance)
+ std::optional<RobotState>
+ convertRobotState(const armem::wm::EntityInstance& instance)
{
arondto::Robot aronRobot;
try
@@ -62,4 +60,4 @@ namespace armarx::armem::robot
return robotState;
}
-} // namespace armarx::armem::robot
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot/robot_conversions.h b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
similarity index 62%
rename from source/RobotAPI/libraries/armem_robot/robot_conversions.h
rename to source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
index 3692ae048dacb94d2f24469f5265e998ab23fb34..0e4f9a4fb3a8858963a23c6f84f679f730405698 100644
--- a/source/RobotAPI/libraries/armem_robot/robot_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
@@ -9,8 +9,8 @@ namespace armarx::armem::wm
class EntityInstance;
}
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state
{
- std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
+ std::optional<description::RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance);
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 54d5823876f19fb00a6e9e6ec57ecc89db0c3a13..7525e9bfd1180c6939ad7283912492caffdea5d1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -22,7 +22,6 @@ armarx_add_library(
RobotAPIInterfaces
RobotStatechartHelpers
RobotAPI::armem_server
- RobotAPI::armem_robot
RobotAPI::armem_robot_state
aroncommon
aroneigenconverter
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index c5f3fe36a067f6969e2c09b238ac1ed473422c2b..6d6091549805226a703b0b93b6e12a2d4dcc8292 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -81,9 +81,9 @@ namespace armarx::armem::server::robot_state
}
void
- Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+ Visu::visualizeRobots(viz::Layer& layer, const armem::robot_state::Robots& robots)
{
- for (const robot::Robot& robot : robots)
+ for (const armem::robot_state::Robot& robot : robots)
{
const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
@@ -103,17 +103,17 @@ namespace armarx::armem::server::robot_state
void
Visu::visualizeFrames(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
{
for (const auto& [robotName, robotFrames] : frames)
{
- Eigen::Affine3f pose = Eigen::Affine3f::Identity();
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
int i = 0;
for (const auto& frame : robotFrames)
{
- Eigen::Affine3f from = pose;
- Eigen::Affine3f to = pose * frame;
+ Eigen::Isometry3f from = pose;
+ Eigen::Isometry3f to = pose * frame;
pose = to;
@@ -126,7 +126,7 @@ namespace armarx::armem::server::robot_state
void
Visu::visualizeFramesIndividual(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
{
std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
@@ -225,7 +225,7 @@ namespace armarx::armem::server::robot_state
<< "\n- " << globalPoses.size() << " global poses"
<< "\n- " << sensorValues.size() << " joint positions";
- const robot::Robots robots =
+ const armem::robot_state::Robots robots =
combine(robotDescriptions, globalPoses, sensorValues, timestamp);
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
@@ -255,7 +255,7 @@ namespace armarx::armem::server::robot_state
viz::Layer& layerForceTorque = layers.emplace_back(arviz.layer("Force Torque"));
std::stringstream log;
- for (const robot::Robot& robot : robots)
+ for (const armem::robot_state::Robot& robot : robots)
{
const std::string& robotName = robot.description.name;
@@ -310,7 +310,7 @@ namespace armarx::armem::server::robot_state
void
Visu::visualizeForceTorque(viz::Layer& layer,
- const robot::Robot& robot,
+ const armem::robot_state::Robot& robot,
const proprioception::SensorValues& sensorValues)
{
const std::string& robotName = robot.description.name;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 2afa9a27f3396939c625050a942925db79129d64..7208496576c3487a093e3bc4a6814a72b32ca13d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -23,13 +23,15 @@
#include <optional>
+#include <ArmarXCore/core/time.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
-#include <RobotAPI/libraries/armem_objects/types.h>
+// #include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
namespace armarx::armem::server::robot_state
@@ -55,22 +57,22 @@ namespace armarx::armem::server::robot_state
private:
void visualizeRun();
- void visualizeOnce(const Time& timestamp);
+ void visualizeOnce(const core::time::DateTime& timestamp);
- static void visualizeRobots(viz::Layer& layer, const robot::Robots& robots);
+ static void visualizeRobots(viz::Layer& layer, const armem::robot_state::Robots& robots);
static void visualizeFrames(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames);
static void visualizeFramesIndividual(
viz::Layer& layerFrames,
- const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+ const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames);
void visualizeForceTorque(viz::Layer& layer,
- const robot::Robot& robot,
+ const armem::robot_state::Robot& robot,
const proprioception::SensorValues& sensorValues);
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
index 997253f25c8bce846e3dddfe8e14aa9f04414d78..71e9cbf253068d19925ae0e5ae43725fa9ffba35 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.cpp
@@ -1,39 +1,37 @@
#include "combine.h"
-#include <RobotAPI/libraries/armem_robot/types.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
+#include <sstream>
#include <ArmarXCore/core/logging/Logging.h>
-#include <sstream>
-
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx::armem::server
+namespace armarx::armem::server::robot_state
{
- robot::Robots
- robot_state::combine(
- const description::RobotDescriptionMap& robotDescriptions,
- const localization::RobotPoseMap& globalPoses,
- const proprioception::SensorValuesMap& sensorValues,
- const armem::Time& timestamp)
+ armem::robot_state::Robots
+ combine(const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::SensorValuesMap& sensorValues,
+ const armem::Time& timestamp)
{
std::stringstream logs;
- robot::Robots robots;
+ armem::robot_state::Robots robots;
for (const auto& [robotName, robotDescription] : robotDescriptions)
{
// Handle missing values gracefully instead of skipping the robot altogether.
- robot::Robot& robot = robots.emplace_back();
+ armem::robot_state::Robot& robot = robots.emplace_back();
robot.description = robotDescription;
- robot.instance = ""; // TODO(fabian.reister): set this properly
- robot.config.timestamp = timestamp;
- robot.config.globalPose = Eigen::Affine3f::Identity();
- robot.config.jointMap = {};
- robot.timestamp = timestamp;
+ robot.instance = ""; // TODO(fabian.reister): set this properly
+ robot.config.timestamp = timestamp;
+ robot.config.globalPose = Eigen::Isometry3f::Identity();
+ robot.config.jointMap = {};
+ robot.timestamp = timestamp;
if (auto it = globalPoses.find(robotName); it != globalPoses.end())
{
@@ -65,4 +63,4 @@ namespace armarx::armem::server
return robots;
}
-}
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
index b96ca240c552426651ed4871779ea747604706cf..328aa586e88f8387d30ff9d0d489183a40809a13 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/combine.h
@@ -24,15 +24,12 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-
namespace armarx::armem::server::robot_state
{
- robot::Robots
- combine(
- const description::RobotDescriptionMap& robotDescriptions,
- const localization::RobotPoseMap& globalPoses,
- const proprioception::SensorValuesMap& sensorValues,
- const armem::Time& timestamp);
+ armem::robot_state::Robots combine(const description::RobotDescriptionMap& robotDescriptions,
+ const localization::RobotPoseMap& globalPoses,
+ const proprioception::SensorValuesMap& sensorValues,
+ const armem::Time& timestamp);
-} // namespace armarx::armem::server::robot_state
+} // 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
index 7a8f6ae0369050000b2a1b8f4df57ec4de4f050a..13587664c03a0c31ab83e8295561372e07630223 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -8,7 +8,7 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
@@ -18,10 +18,11 @@
#include <RobotAPI/libraries/armem/core/aron_conversions.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>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
@@ -45,7 +46,7 @@ namespace armarx::armem::server::robot_state::description
}
- void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
+ void Segment::commitRobotDescription(const armem::robot_state::description::RobotDescription& robotDescription)
{
const Time now = Time::Now();
@@ -61,7 +62,7 @@ namespace armarx::armem::server::robot_state::description
update.arrivedTime = update.referencedTime = update.sentTime = now;
arondto::RobotDescription dto;
- robot::toAron(dto, robotDescription);
+ toAron(dto, robotDescription);
update.instancesData =
{
dto.toAron()
@@ -83,7 +84,13 @@ namespace armarx::armem::server::robot_state::description
const std::string robotFilename = kinematicUnit->getRobotFilename();
const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ // const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ const std::string package = simox::alg::split(robotFilename, "/").front();
+
+ ARMARX_CHECK_NOT_EMPTY(package);
+
+ ARMARX_INFO << VAROUT(package);
+ ARMARX_INFO << VAROUT(robotFilename);
// make sure that the relative path is without the 'package/' prefix
const std::string robotFileRelPath = [&robotFilename, &package]()-> std::string {
@@ -100,7 +107,7 @@ namespace armarx::armem::server::robot_state::description
ARMARX_INFO << "Robot description '" << VAROUT(robotFileRelPath) << "' found in package " << package;
- const robot::RobotDescription robotDescription
+ const armem::robot_state::description::RobotDescription robotDescription
{
.name = kinematicUnit->getRobotName(),
.xml = {package, robotFileRelPath}
@@ -139,7 +146,7 @@ namespace armarx::armem::server::robot_state::description
segmentPtr->forEachEntity([this, &robotDescriptions](const wm::Entity & entity)
{
const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = robot::convertRobotDescription(entityInstance);
+ const auto description = ::armarx::armem::robot_state::convertRobotDescription(entityInstance);
if (description)
{
ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index fb617db14ae43f7b7f55d6ed8f88223f748a9ad3..9ea6ec26e0e6dc4f9ff89e30425166b3cdf66159 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -22,9 +22,9 @@
#pragma once
// STD / STL
-#include <string>
-#include <optional>
#include <mutex>
+#include <optional>
+#include <string>
#include <unordered_map>
// ArmarX
@@ -34,8 +34,8 @@
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
// Aron
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::server::robot_state::description
{
@@ -45,7 +45,6 @@ namespace armarx::armem::server::robot_state::description
using Base = segment::SpecializedCoreSegment;
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
@@ -58,12 +57,11 @@ namespace armarx::armem::server::robot_state::description
private:
-
- void commitRobotDescription(const robot::RobotDescription& robotDescription);
+ void commitRobotDescription(
+ const armem::robot_state::description::RobotDescription& robotDescription);
void updateRobotDescription();
RobotUnitInterfacePrx robotUnit;
-
};
-} // namespace armarx::armem::server::robot_state::description
+} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
index 63f24d0f46a156adc023b3b64b4f27d344b151c6..bb74546dd41aacb7460101f146fdf7145cfdc0eb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
@@ -16,10 +16,10 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.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>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
index e8471563d640800a44d4516d6eccdd7d7e355ccd..3b6d0042909223d93e7c69555ea96a2fe89d06f6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
@@ -29,7 +29,7 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
index 1b9c39543b21587f30a32feb2dfbbcac4fb5e257..f4b4a871bb499eb0d857a0cde0227a1f3b10d813 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/forward_declarations.h
@@ -28,8 +28,6 @@
#include <Eigen/Geometry>
-
-
namespace armarx::armem::arondto
{
struct Transform;
@@ -38,18 +36,22 @@ namespace armarx::armem::arondto
struct JointState;
class RobotDescription;
-}
+} // namespace armarx::armem::arondto
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state
{
- struct RobotDescription;
+ namespace description
+ {
+ struct RobotDescription;
+ using RobotDescriptions = std::vector<RobotDescription>;
+
+ } // namespace description
struct RobotState;
struct Robot;
using Robots = std::vector<Robot>;
- using RobotDescriptions = std::vector<RobotDescription>;
using RobotStates = std::vector<RobotState>;
-}
+} // namespace armarx::armem::robot_state
namespace armarx::armem::robot_state
{
@@ -57,20 +59,21 @@ namespace armarx::armem::robot_state
struct Transform;
struct TransformHeader;
-}
+} // namespace armarx::armem::robot_state
namespace armarx::armem::server::robot_state::description
{
- using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
+ using RobotDescriptionMap =
+ std::unordered_map<std::string, armarx::armem::robot_state::description::RobotDescription>;
class Segment;
-}
+} // namespace armarx::armem::server::robot_state::description
namespace armarx::armem::server::robot_state::localization
{
- using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
- using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Affine3f>>;
+ using RobotPoseMap = std::unordered_map<std::string, Eigen::Isometry3f>;
+ using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>;
class Segment;
-}
+} // namespace armarx::armem::server::robot_state::localization
namespace armarx::armem::server::robot_state::proprioception
{
@@ -81,5 +84,4 @@ namespace armarx::armem::server::robot_state::proprioception
using ForceTorqueValuesMap = std::unordered_map<std::string, ForceTorqueValues>;
using SensorValuesMap = std::unordered_map<std::string, SensorValues>;
class Segment;
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index a503bae532af41ee91809d394c22cd74f4aab071..7384f90ed1af0f6237f98c570c4b855a31cef583 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -19,8 +19,8 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -80,8 +80,8 @@ namespace armarx::armem::server::robot_state::localization
RobotFramePoseMap
Segment::getRobotFramePoses(const armem::Time& timestamp) const
{
- using common::robot_state::localization::TransformHelper;
- using common::robot_state::localization::TransformQuery;
+ using ::armarx::armem::robot_state::localization::TransformHelper;
+ using ::armarx::armem::robot_state::localization::TransformQuery;
RobotFramePoseMap frames;
for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
@@ -126,8 +126,8 @@ namespace armarx::armem::server::robot_state::localization
RobotPoseMap
Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
{
- using common::robot_state::localization::TransformHelper;
- using common::robot_state::localization::TransformQuery;
+ using ::armarx::armem::robot_state::localization::TransformHelper;
+ using ::armarx::armem::robot_state::localization::TransformQuery;
RobotPoseMap robotGlobalPoses;
for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
@@ -160,7 +160,7 @@ namespace armarx::armem::server::robot_state::localization
}
- bool Segment::commitTransform(const armarx::armem::robot_state::Transform& transform)
+ bool Segment::commitTransform(const armarx::armem::robot_state::localization::Transform& transform)
{
Commit commit;
commit.add(makeUpdate(transform));
@@ -170,7 +170,7 @@ namespace armarx::armem::server::robot_state::localization
}
- bool Segment::commitTransformLocking(const armarx::armem::robot_state::Transform& transform)
+ bool Segment::commitTransformLocking(const armarx::armem::robot_state::localization::Transform& transform)
{
Commit commit;
commit.add(makeUpdate(transform));
@@ -180,7 +180,7 @@ namespace armarx::armem::server::robot_state::localization
}
- EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::Transform& transform) const
+ EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::localization::Transform& transform) const
{
const armem::Time& timestamp = transform.header.timestamp;
const MemoryID providerID = segmentPtr->id().withProviderSegmentName(transform.header.agent);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index 0f1fe71345bf1d2767f5cfc7b0c268e1f41e7eba..b974ff31af81496c7d320953f16ae57900a7c897 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -36,6 +36,7 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::server::robot_state::localization
@@ -62,13 +63,13 @@ namespace armarx::armem::server::robot_state::localization
RobotFramePoseMap getRobotFramePoses(const armem::Time& timestamp) const;
RobotFramePoseMap getRobotFramePosesLocking(const armem::Time& timestamp) const;
- bool commitTransform(const armem::robot_state::Transform& transform);
- bool commitTransformLocking(const armem::robot_state::Transform& transform);
+ bool commitTransform(const armem::robot_state::localization::Transform& transform);
+ bool commitTransformLocking(const armem::robot_state::localization::Transform& transform);
private:
- EntityUpdate makeUpdate(const armem::robot_state::Transform& transform) const;
+ EntityUpdate makeUpdate(const armem::robot_state::localization::Transform& transform) const;
PredictionResult predictLinear(const PredictionRequest& request);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 5c4580c4c8c5d4540b0867365c657807d0502a7d..611257b20771979a763daf62202b3aa96f244ceb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -98,7 +98,7 @@ namespace armarx::armem::server::robot_state::proprioception
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
- for (const armem::robot_state::Transform& transform : update.localization)
+ for (const armem::robot_state::localization::Transform& transform : update.localization)
{
localizationSegment.doLocked(
[&localizationSegment, &transform]()
@@ -210,7 +210,7 @@ namespace armarx::armem::server::robot_state::proprioception
return update;
}
- armem::robot_state::Transform
+ armem::robot_state::localization::Transform
RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
const Time& timestamp) const
{
@@ -219,11 +219,11 @@ namespace armarx::armem::server::robot_state::proprioception
const Eigen::Vector3f& relPose = platform.relativePosition;
- Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ Eigen::Isometry3f odometryPose = Eigen::Isometry3f::Identity();
odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
- armem::robot_state::Transform transform;
+ armem::robot_state::localization::Transform transform;
transform.header.parentFrame = armarx::OdometryFrame;
transform.header.frame = armarx::armem::robot_state::constants::robotRootNodeName;
transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 27922266466b6156bb8f217dc887699a193027d9..917a122df0f8fda9b3cf875f228a2bd63e86e4c2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -72,14 +72,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
armem::Commit proprioception;
armem::Commit exteroception;
- std::vector<armem::robot_state::Transform> localization;
+ std::vector<armem::robot_state::localization::Transform> localization;
};
Update buildUpdate(const RobotUnitData& dataQueue);
private:
- armem::robot_state::Transform
+ armem::robot_state::localization::Transform
getTransform(
const aron::data::DictPtr& platformData,
const Time& timestamp) const;
diff --git a/source/RobotAPI/libraries/armem_robot/types.cpp b/source/RobotAPI/libraries/armem_robot_state/types.cpp
similarity index 59%
rename from source/RobotAPI/libraries/armem_robot/types.cpp
rename to source/RobotAPI/libraries/armem_robot_state/types.cpp
index 6f09eee1acef62472a1e8cc0027f49c9db6efe46..055973126ac840dce8208f05917d45a8c15e0771 100644
--- a/source/RobotAPI/libraries/armem_robot/types.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/types.cpp
@@ -2,23 +2,27 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-namespace armarx::armem::robot
+namespace armarx::armem::robot_state::description
{
- std::ostream& operator<<(std::ostream &os, const RobotDescription &rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const RobotDescription& rhs)
{
os << "RobotDescription { name: '" << rhs.name << "', xml: '" << rhs.xml << "' }";
return os;
}
+} // namespace armarx::armem::robot_state::description
+namespace armarx::armem::robot_state
+{
- std::string Robot::name() const
+ std::string
+ Robot::name() const
{
ARMARX_CHECK_NOT_EMPTY(description.name) << "The robot name must be set!";
ARMARX_CHECK_NOT_EMPTY(instance) << "The robot instance name must be provided!";
return description.name + "/" + instance;
}
-
-}
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index b6d2b67dec2b28425e7b3dd4b0630fc39df02017..ef1b9c1e9fff8e8cb09cdf1dfa2c869231ec0423 100644
--- a/source/RobotAPI/libraries/armem_robot_state/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -21,36 +21,137 @@
#pragma once
+#include <filesystem>
+#include <map>
+#include <vector>
+
#include <Eigen/Geometry>
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot_state
{
- struct TransformHeader
+
+ namespace description
{
- std::string parentFrame;
- std::string frame;
+ struct RobotDescription
+ {
+ // DateTime timestamp;
+
+ std::string name;
+ PackagePath xml{"", std::filesystem::path("")};
+ };
- std::string agent;
+ using RobotDescriptions = std::vector<RobotDescription>;
- armem::Time timestamp;
+ std::ostream& operator<<(std::ostream& os, const RobotDescription& rhs);
+
+
+ } // namespace description
+
+ struct Twist
+ {
+ Eigen::Vector3f linear{Eigen::Vector3f::Zero()};
+ Eigen::Vector3f angular{Eigen::Vector3f::Zero()};
};
- struct Transform
+ struct PlatformState
{
- TransformHeader header;
+ Twist twist;
+ };
+
+ namespace proprioception
+ {
+
+ struct ForceTorque
+ {
+ Eigen::Vector3f force;
+ Eigen::Vector3f gravityCompensatedForce;
+ Eigen::Vector3f torque;
+ Eigen::Vector3f gravityCompensatedTorque;
+ };
+
+ struct JointState
+ {
+ std::string name;
+ float position;
+ };
+
+ } // namespace proprioception
+
+ namespace exteroception
+ {
+
+ struct ToF
+ {
+ using DepthMatrixType = Eigen::MatrixXf; // FIXME uint16
+ using DepthSigmaMatrixType = Eigen::MatrixXf; // FIXME uint8
+ using TargetDetectedMatrixType = Eigen::MatrixXf; // FIXME uint8
+
+ DepthMatrixType depth;
+ DepthSigmaMatrixType sigma;
+ TargetDetectedMatrixType targetDetected;
- Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+ std::string frame;
+ };
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ } // namespace exteroception
+
+ struct RobotState
+ {
+ using JointMap = std::map<std::string, float>;
+ using Pose = Eigen::Isometry3f;
+
+ DateTime timestamp;
+
+ Pose globalPose;
+ JointMap jointMap;
+
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
};
- struct JointState
+ struct Robot
{
- std::string name;
- float position;
+ description::RobotDescription description;
+ std::string instance;
+
+ RobotState config;
+
+ DateTime timestamp;
+
+ std::string name() const;
};
-} // namespace armarx::armem::robot_state
+ using Robots = std::vector<Robot>;
+ using RobotStates = std::vector<RobotState>;
+
+ namespace localization
+ {
+
+ struct TransformHeader
+ {
+ std::string parentFrame;
+ std::string frame;
+
+ std::string agent;
+
+ armem::Time timestamp;
+ };
+
+ struct Transform
+ {
+ TransformHeader header;
+
+ Eigen::Isometry3f transform = Eigen::Isometry3f::Identity();
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+ } // namespace localization
+
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.cpp b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
index 9f4b84fdfbee87a4ca768837aa41039f1970115e..5e35c27c75eb84e0493008290879dcecec22efd1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/utils.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
@@ -2,10 +2,10 @@
namespace armarx::armem::robot_state
{
- armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc)
+ armarx::armem::MemoryID makeMemoryID(const description::RobotDescription& desc)
{
return MemoryID("RobotState/Description")
.withProviderSegmentName(desc.name)
.withEntityName("description");
}
-}
\ No newline at end of file
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.h b/source/RobotAPI/libraries/armem_robot_state/utils.h
index 9b495f91f30544fbdb2cfc2f68ee10bb111e1903..0119abe3a9377385197addd404331fd3162b6fec 100644
--- a/source/RobotAPI/libraries/armem_robot_state/utils.h
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.h
@@ -1,9 +1,9 @@
#pragma once
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::robot_state
{
- armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc);
+ armarx::armem::MemoryID makeMemoryID(const description::RobotDescription& desc);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
index 3b05caec20fae2d7a43b406f043e4c2da703923c..9923f490f5e439367946230e5584cc4a865c9e2c 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
@@ -22,12 +22,10 @@ armarx_add_test(
TEST_FILE
aronOperatorTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
aron
aroncommon
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -39,9 +37,10 @@ armarx_add_test(
TEST_FILE
aronCodeGenerationTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
+ Eigen3::Eigen
ARON_FILES
aron/AnyTest.xml
aron/TemplateTest.xml
@@ -62,8 +61,6 @@ armarx_add_test(
aron/OptionalTest.xml
aron/DtoTest.xml
INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
- ${Eigen3_INCLUDE_DIR}
${IVT_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
@@ -78,13 +75,11 @@ armarx_add_test(
TEST_FILE
aronNavigateTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/NaturalIKTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -96,14 +91,12 @@ armarx_add_test(
TEST_FILE
aronExtendsTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/BaseClassTest.xml
aron/DerivedClassTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -115,13 +108,11 @@ armarx_add_test(
TEST_FILE
aronConversionTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/HumanPoseTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
######################
@@ -133,14 +124,12 @@ armarx_add_test(
TEST_FILE
aronJsonExportTest.cpp
LIBS
- SimoxUtility # Simox::SimoxUtility
+ Simox::SimoxUtility
ArmarXCore
RobotAPI::aron
ARON_FILES
aron/OptionalTest.xml
aron/MatrixTest.xml
- INCLUDE_DIRECTORIES
- ${Simox_INCLUDE_DIR}
)
########################
@@ -152,13 +141,14 @@ armarx_add_test(
# TEST_FILE
# aronRandomizedTest.cpp
# LIBS
-# SimoxUtility # Simox::SimoxUtility
+# Simox::SimoxUtility
# ArmarXCore
# RobotAPI::aron
# ivt
# ivtopencv
# ${PCL_COMMON_LIBRARIES}
# aronjsonconverter
+# Eigen3::Eigen
# ARON_FILES
# aron/DictTest.xml
# aron/MatrixTest.xml
@@ -176,8 +166,6 @@ armarx_add_test(
# aron/OptionalTest.xml
# aron/ImageTest.xml
# INCLUDE_DIRECTORIES
-# ${Simox_INCLUDE_DIR}
-# ${Eigen3_INCLUDE_DIR}
# ${IVT_INCLUDE_DIRS}
# ${OpenCV_INCLUDE_DIRS}
# ${PCL_INCLUDE_DIRS}
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 7756a2cd5be8edad7ee10e8fdcdc25b4501cb008..5cf0313b9a1ac623134646c345d9fdbc85bf0faf 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -1,22 +1,18 @@
armarx_set_target("RobotAPI Core Library: RobotAPICore")
find_package(Eigen3 QUIET)
-find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+find_package(Simox ${ArmarX_Simox_VERSION} REQUIRED)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-if (Eigen3_FOUND AND Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIRS})
- include_directories(SYSTEM ${Eigen3_INCLUDE_DIR})
-endif()
-
set(LIB_NAME RobotAPICore)
set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreEigen3Variants
- VirtualRobot
- Saba
- SimDynamics
+ Simox::VirtualRobot
+ # Simox::Saba
+ # Simox::SimDynamics
+ Eigen3::Eigen
)
set(LIB_FILES