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 976d5bf9abb8a7d7586a0501568e6c8240c1e45c..c4e703531a9508757a00e6c1dbf5429c7436b470 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 e8e21735dde6ccba6a1979dd10a676fe5801d8f3..cf66b17e68c9d185ffd819dbb791707bdf65d6f7 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 f5dac4035dfb4c8ccab3e82e78af035d64fd981f..e0dca8f3ebd68e682f7bff2bcc759c6bb2673759 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 85370141f880a72b26039c0af0c765f929e6a902..57ecaf8de87598880229489908edb300431b96fa 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())