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 5ea6d2cd90c0412c2b9243262ffb3166f5ce9f13..4da91df52e77ecd83fe4c061d0a969bce8bae32f 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp @@ -4,6 +4,7 @@ #include <optional> #include "ArmarXCore/core/exceptions/local/ExpressionException.h" +#include "RobotAPI/libraries/armem_robot/types.h" #include <ArmarXCore/core/exceptions/LocalException.h> #include <ArmarXCore/core/logging/Logging.h> #include <ArmarXCore/core/PackagePath.h> @@ -23,6 +24,7 @@ namespace fs = ::std::filesystem; namespace armarx::armem::robot_state { + RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) : memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem) @@ -197,6 +199,40 @@ namespace armarx::armem::robot_state return getRobotJointState(qResult.memory, description.name); } + + + std::optional<robot::PlatformState> + RobotReader::queryPlatformState(const robot::RobotDescription& description, + const armem::Time& timestamp) const + { + // TODO(fabian.reister): how to deal with multiple providers? + + // 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) + .providerSegments().withName(description.name) // agent + .entities().all() // TODO + .snapshots().latest(); + // clang-format on + + const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput()); + + ARMARX_DEBUG << "Lookup result in reader: " << qResult; + + if (not qResult.success) /* c++20 [[unlikely]] */ + { + ARMARX_WARNING << qResult.errorMessage; + return std::nullopt; + } + + return getRobotPlatformState(qResult.memory, description.name); + } + std::optional<robot::RobotState::Pose> RobotReader::queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const @@ -307,6 +343,34 @@ namespace armarx::armem::robot_state return jointMap; } + std::optional<robot::PlatformState> + RobotReader::getRobotPlatformState (const armarx::armem::wm::Memory& memory, + const std::string& name) const + { + std::optional<robot::PlatformState> platformState; + + // clang-format off + const armem::wm::CoreSegment& coreSegment = memory + .getCoreSegment(properties.proprioceptionCoreSegment); + // clang-format on + + coreSegment.forEachEntity([&platformState](const wm::Entity & entity) + { + const auto& entityInstance = entity.getLatestSnapshot().getInstance(0); + + const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance); + ARMARX_CHECK(proprioception.has_value()); + + platformState = robot::PlatformState(); // initialize optional + robot::fromAron(proprioception->platform, platformState.value()); + + }); + + return platformState; + } + + + std::optional<robot::RobotDescription> RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const 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 b3f71e59ac2b33c19ff97fd0c238f5540503b6ee..e24bc65fa8a9ca14213bfe79007fc8519f02d176 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h @@ -72,14 +72,24 @@ namespace armarx::armem::robot_state queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const; + std::optional<robot::PlatformState> + queryPlatformState(const robot::RobotDescription& description, + const armem::Time& timestamp) const; + private: std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const; + std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const; + std::optional<robot::RobotState::JointMap> getRobotJointState(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; + + struct Properties { std::string memoryName = "RobotState";