21 #include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
22 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
23 #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
29 namespace fs = ::std::filesystem;
34 properties(r.properties),
35 propertyPrefix(r.propertyPrefix),
36 memoryReader(r.memoryReader),
37 transformReader(r.transformReader)
50 transformReader.
connect(memoryNameSystem);
77 return get(*description, timestamp);
87 .timestamp = timestamp};
128 ARMARX_INFO <<
"Retrying to query robot state after failure";
132 robot.
config = std::move(*state);
137 std::optional<description::RobotDescription>
156 if (not memoryReader)
158 ARMARX_WARNING <<
"Memory reader is null. Did you forget to call "
159 "RobotReader::connect() in onConnectComponent()?";
165 std::scoped_lock l(memoryReaderMutex);
170 if (not qResult.success)
175 return getRobotDescription(qResult.memory, name);
185 std::optional<RobotState>
189 std::optional<RobotState> robotState =
queryJointState(robotName, timestamp);
196 ARMARX_VERBOSE <<
"Failed to query global pose for robot " << robotName;
199 robotState->globalPose = *globalPose;
205 std::optional<RobotState>
211 if (not proprioception.has_value())
213 ARMARX_VERBOSE <<
"Failed to query proprioception for robot '" << robotName
217 const auto jointMap = proprioception->joints.position;
221 .jointMap = jointMap,
222 .proprioception = proprioception};
225 std::optional<::armarx::armem::robot_state::localization::Transform>
234 .timestamp = timestamp}};
252 std::optional<armarx::armem::arondto::Proprioception>
261 ARMARX_DEBUG <<
"Querying robot description for robot: " << robotName;
273 std::scoped_lock l(memoryReaderMutex);
278 if (not qResult.success)
284 return getRobotProprioception(qResult.memory, robotName);
301 ARMARX_DEBUG <<
"Querying robot joint states for robot: `" << robotName
302 <<
"` on time interval [" << begin <<
"," << end <<
"]";
314 std::scoped_lock l(memoryReaderMutex);
319 if (not qResult.success)
325 return getRobotJointStates(qResult.memory, robotName);
335 std::optional<PlatformState>
344 ARMARX_DEBUG <<
"Querying robot description for robot: " << robotName;
356 std::scoped_lock l(memoryReaderMutex);
361 if (not qResult.success)
367 return getRobotPlatformState(qResult.memory, robotName);
377 std::optional<RobotState::Pose>
400 std::optional<RobotState>
402 const std::string& name)
const
407 .getProviderSegment(name);
411 if (providerSegment.
empty())
427 if (instance ==
nullptr)
438 template <
typename AronClass>
439 std::optional<AronClass>
447 return AronClass::FromAron(item.
data());
455 std::optional<armarx::armem::arondto::Proprioception>
457 const std::string& name)
const
464 std::optional<armarx::armem::arondto::Proprioception> proprioception;
476 proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
479 return proprioception;
484 const std::string& name)
const
490 const armem::wm::CoreSegment& coreSegment = memory
494 coreSegment.forEachEntity(
495 [&jointTrajectory](
const wm::Entity& entity)
497 entity.forEachSnapshot(
498 [&](
const auto& snapshot)
500 if (not snapshot.hasInstance(0))
505 const auto& entityInstance = snapshot.getInstance(0);
507 const auto proprioception =
508 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
513 jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
517 ARMARX_INFO <<
"Joint trajectory with " << jointTrajectory.size() <<
" elements";
519 return jointTrajectory;
523 std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
531 ARMARX_DEBUG <<
"Querying force torques description for robot: " << robotName;
543 std::scoped_lock l(memoryReaderMutex);
548 if (not qResult.success)
554 return getForceTorque(qResult.memory, robotName);
565 std::optional<std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
574 ARMARX_DEBUG <<
"Querying force torques description for robot: " << robotName;
586 std::scoped_lock l(memoryReaderMutex);
591 if (not qResult.success)
597 return getForceTorques(qResult.memory, robotName);
607 std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
614 ARMARX_DEBUG <<
"Querying ToF data for robot: " << robotName;
626 std::scoped_lock l(memoryReaderMutex);
631 if (not qResult.success)
637 return getToF(qResult.memory, robotName);
647 std::optional<PlatformState>
649 const std::string& name)
const
651 std::optional<PlatformState> platformState;
668 const auto proprioception =
669 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
673 fromAron(proprioception->platform, platformState.value());
676 return platformState;
692 throw LocalException(
"Unknown hand name `" + name +
"`!");
695 std::map<RobotReader::Hand, proprioception::ForceTorque>
697 const std::string& name)
const
699 std::map<RobotReader::Hand, proprioception::ForceTorque> forceTorques;
716 const auto proprioception =
717 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
720 for (
const auto& [handName, dtoFt] : proprioception->forceTorque)
722 proprioception::ForceTorque forceTorque;
723 fromAron(dtoFt, forceTorque);
725 const auto hand = fromHandName(handName);
726 forceTorques.emplace(hand, forceTorque);
733 std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
735 const std::string& name)
const
737 std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>> forceTorques;
740 const armem::wm::CoreSegment& coreSegment = memory
744 coreSegment.forEachInstance(
747 const auto proprioception =
748 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
752 for (
const auto& [handName, dtoFt] : proprioception->forceTorque)
754 proprioception::ForceTorque forceTorque;
758 forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
765 std::map<RobotReader::Hand, exteroception::ToF>
768 std::map<RobotReader::Hand, exteroception::ToF> tofs;
771 const armem::wm::CoreSegment& coreSegment = memory
775 coreSegment.forEachEntity(
776 [&tofs](
const wm::Entity& entity)
780 if (not entity.getLatestSnapshot().hasInstance(0))
784 const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
786 const auto exteroception =
787 tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
791 for (
const auto& [handName, dtoFt] : exteroception->tof)
793 ARMARX_DEBUG <<
"Processing ToF element for hand `" << handName <<
"`";
795 exteroception::ToF tof;
796 fromAron(dtoFt, tof);
798 const auto hand = fromHandName(handName);
799 tofs.emplace(hand, tof);
806 std::optional<description::RobotDescription>
808 const std::string& name)
const
811 const armem::wm::ProviderSegment& providerSegment = memory
813 .getProviderSegment(name);
819 if (instance ==
nullptr)
821 ARMARX_VERBOSE <<
"No entity snapshots found in provider segment `" << name <<
"`";
828 std::vector<description::RobotDescription>
831 const armem::wm::CoreSegment& coreSegment =
834 std::vector<description::RobotDescription> descriptions;
836 coreSegment.forEachInstance(
839 if (
const std::optional<description::RobotDescription> desc =
842 descriptions.push_back(desc.value());
849 std::vector<description::RobotDescription>
867 if (not memoryReader)
869 ARMARX_WARNING <<
"Memory reader is null. Did you forget to call "
870 "RobotReader::connect() in onConnectComponent()?";
876 std::scoped_lock l(memoryReaderMutex);
881 if (not qResult.success)
886 return getRobotDescriptions(qResult.memory);