From 8626c97b31aedd680cf033a08363bf51cdcbca99 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 2 Sep 2021 20:33:08 +0200
Subject: [PATCH] fixing armem: ArticulatedObjectReader, RobotReader (changes
to memory data types ...)
---
.../ArticulatedObjectReader.cpp | 5 +-
.../client/articulated_object/Reader.cpp | 47 +++++++++++++++----
.../client/articulated_object/Reader.h | 2 +-
.../client/common/RobotReader.cpp | 27 ++++++++---
4 files changed, 64 insertions(+), 17 deletions(-)
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 976d5bf9a..c4e703531 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -43,12 +43,15 @@ namespace armarx::armem::articulated_object
return nullptr;
}
+ ARMARX_DEBUG << "Object " << name << " available";
+
auto obj =
VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
VirtualRobot::RobotIO::eStructure);
if (not obj)
{
+ ARMARX_WARNING << "Failed to load object: " << name;
return nullptr;
}
@@ -58,4 +61,4 @@ namespace armarx::armem::articulated_object
return obj;
}
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
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 e8e21735d..cf66b17e6 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -2,7 +2,9 @@
#include <mutex>
#include <optional>
+#include <Eigen/src/Geometry/Transform.h>
+#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -188,7 +190,7 @@ namespace armarx::armem::articulated_object
// clang-format off
qb
.coreSegments().withName(properties.coreClassSegmentName)
- .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+ .providerSegments().withName(properties.providerName)
.entities().all() // withName(name)
.snapshots().latest();
// clang-format on
@@ -231,11 +233,38 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- return getRobotState(qResult.memory);
+ return getArticulatedObjectState(qResult.memory);
+ }
+
+
+ std::optional<robot::RobotState> convertToRobotState(const armem::wm::EntityInstance& instance)
+ {
+ armarx::armem::arondto::ObjectInstance aronObjectInstance;
+ try
+ {
+ aronObjectInstance.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to ObjectInstance failed!";
+ return std::nullopt;
+ }
+
+ objpose::ObjectPose objectPose;
+ objpose::fromAron(aronObjectInstance.pose, objectPose);
+
+ robot::RobotState robotState
+ {
+ .timestamp = objectPose.timestamp,
+ .globalPose = Eigen::Affine3f(objectPose.objectPoseRobot),
+ .jointMap = objectPose.objectJointValues
+ };
+
+ return robotState;
}
std::optional<robot::RobotState>
- Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ Reader::getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -247,14 +276,16 @@ namespace armarx::armem::articulated_object
{
instance = i;
});
+
if (instance.has_value())
{
- return robot::convertRobotState(instance.value());
- }
- else
- {
- return std::nullopt;
+ return convertToRobotState(instance.value());
+ // return robot::convertRobotState(instance.value());
}
+
+ ARMARX_FATAL << "Failed to obtain robot state";
+ return std::nullopt;
+
}
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 f5dac4035..e0dca8f3e 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -66,7 +66,7 @@ namespace armarx::armem::articulated_object
protected:
std::optional<robot::RobotState>
- getRobotState(const armarx::armem::wm::Memory& memory) const;
+ getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const;
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory) const;
std::vector<robot::RobotDescription>
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 85370141f..57ecaf8de 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -3,6 +3,7 @@
#include <mutex>
#include <optional>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
@@ -15,6 +16,7 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace fs = ::std::filesystem;
@@ -172,6 +174,8 @@ namespace armarx::armem::robot_state
// Query all entities from provider.
armem::client::query::Builder qb;
+ ARMARX_DEBUG << "Querying robot description for robot: " << description;
+
// clang-format off
qb
.coreSegments().withName(properties.proprioceptionCoreSegment)
@@ -276,14 +280,23 @@ namespace armarx::armem::robot_state
{
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- if (not jointState)
- {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- return;
- }
+ const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+ ARMARX_CHECK(proprioception.has_value());
+
+ const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+
+
+
+ // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+ // if (not jointState)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ // return;
+ // }
+
+ jointMap = joints.position;
- jointMap.emplace(jointState->name, jointState->position);
+ // jointMap.emplace(jointState->name, jointState->position);
});
if (jointMap.empty())
--
GitLab