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 95e9f257c5e25d1bfc0449b4f1c1fb134cd2a3a7..16c83498cd59f65418d00f649031b750d4d44969 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp @@ -170,7 +170,6 @@ namespace armarx::armem::articulated_object // Query all entities from provider. armem::client::query::Builder qb; - if (providerName.has_value()) // query single provider { @@ -262,7 +261,7 @@ namespace armarx::armem::articulated_object ARMARX_TRACE; // Query all entities from provider. - armem::client::query::Builder qb; + armem::client::query::Builder qb; if (not providerName.has_value()) { @@ -340,17 +339,16 @@ namespace armarx::armem::articulated_object .getCoreSegment(objects::constants::CoreInstanceSegmentName); // clang-format on - std::optional<wm::EntityInstance> instance; - coreSegment.forEachInstance([&instance](const wm::EntityInstance& i) { instance = i; }); - - if (instance.has_value()) + try { - return convertToRobotState(instance.value()); - // return robot::convertRobotState(instance.value()); + const wm::EntityInstance& instance = coreSegment.getLatestInstance(); + return convertToRobotState(instance); + } + catch (...) + { + ARMARX_FATAL << "Failed to obtain robot state"; + return std::nullopt; } - - ARMARX_FATAL << "Failed to obtain robot state"; - return std::nullopt; } std::optional<robot::RobotDescription>