21 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
25 #include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
26 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
30 namespace fs = ::std::filesystem;
35 properties(r.properties),
36 propertyPrefix(r.propertyPrefix),
37 memoryReader(r.memoryReader),
38 transformReader(r.transformReader)
51 transformReader.
connect(memoryNameSystem);
67 std::optional<robot::Robot>
78 return get(*description, timestamp);
87 .timestamp = timestamp};
128 ARMARX_INFO <<
"Retrying to query robot state after failure";
132 obj.
config = std::move(*state);
137 std::optional<robot::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<robot::RobotState>
191 if (not proprioception.has_value())
197 const auto jointMap = proprioception->joints.position;
207 .globalPose = *globalPose,
208 .jointMap = jointMap,
209 .proprioception = proprioception};
212 std::optional<::armarx::armem::robot_state::Transform>
220 .agent = description.
name,
221 .timestamp = timestamp}};
239 std::optional<armarx::armem::arondto::Proprioception>
248 ARMARX_DEBUG <<
"Querying robot description for robot: " << description;
260 std::scoped_lock l(memoryReaderMutex);
265 if (not qResult.success)
271 return getRobotProprioception(qResult.memory, description.name);
288 ARMARX_DEBUG <<
"Querying robot joint states for robot: `" << description
289 <<
"` on time interval [" << begin <<
"," << end <<
"]";
301 std::scoped_lock l(memoryReaderMutex);
306 if (not qResult.success)
312 return getRobotJointStates(qResult.memory, description.
name);
322 std::optional<robot::PlatformState>
331 ARMARX_DEBUG <<
"Querying robot description for robot: " << description;
343 std::scoped_lock l(memoryReaderMutex);
348 if (not qResult.success)
354 return getRobotPlatformState(qResult.memory, description.name);
364 std::optional<robot::RobotState::Pose>
387 std::optional<robot::RobotState>
389 const std::string& name)
const
394 .getProviderSegment(name);
398 if (providerSegment.
empty())
414 if (instance ==
nullptr)
425 template <
typename AronClass>
426 std::optional<AronClass>
434 return AronClass::FromAron(item.
data());
442 std::optional<armarx::armem::arondto::Proprioception>
444 const std::string& name)
const
451 std::optional<armarx::armem::arondto::Proprioception> proprioception;
463 proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
466 return proprioception;
471 const std::string& name)
const
477 const armem::wm::CoreSegment& coreSegment = memory
481 coreSegment.forEachEntity(
482 [&jointTrajectory](
const wm::Entity& entity)
484 entity.forEachSnapshot(
485 [&](
const auto& snapshot)
487 if (not snapshot.hasInstance(0))
492 const auto& entityInstance = snapshot.getInstance(0);
494 const auto proprioception =
495 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
500 jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
504 ARMARX_INFO <<
"Joint trajectory with " << jointTrajectory.size() <<
" elements";
506 return jointTrajectory;
510 std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
518 ARMARX_DEBUG <<
"Querying force torques description for robot: " << description;
530 std::scoped_lock l(memoryReaderMutex);
535 if (not qResult.success)
541 return getForceTorque(qResult.memory, description.name);
552 std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
561 ARMARX_DEBUG <<
"Querying force torques description for robot: " << description;
573 std::scoped_lock l(memoryReaderMutex);
578 if (not qResult.success)
584 return getForceTorques(qResult.memory, description.name);
594 std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
601 ARMARX_DEBUG <<
"Querying ToF data for robot: " << description;
613 std::scoped_lock l(memoryReaderMutex);
618 if (not qResult.success)
624 return getToF(qResult.memory, description.name);
634 std::optional<robot::PlatformState>
636 const std::string& name)
const
638 std::optional<robot::PlatformState> platformState;
655 const auto proprioception =
656 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
660 fromAron(proprioception->platform, platformState.value());
663 return platformState;
679 throw LocalException(
"Unknown hand name `" + name +
"`!");
682 std::map<RobotReader::Hand, robot::ForceTorque>
684 const std::string& name)
const
686 std::map<RobotReader::Hand, robot::ForceTorque> forceTorques;
703 const auto proprioception =
704 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
707 for (
const auto& [handName, dtoFt] : proprioception->forceTorque)
709 robot::ForceTorque forceTorque;
710 fromAron(dtoFt, forceTorque);
712 const auto hand = fromHandName(handName);
713 forceTorques.emplace(hand, forceTorque);
720 std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
722 const std::string& name)
const
724 std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>> forceTorques;
727 const armem::wm::CoreSegment& coreSegment = memory
731 coreSegment.forEachInstance(
734 const auto proprioception =
735 tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
739 for (
const auto& [handName, dtoFt] : proprioception->forceTorque)
741 robot::ForceTorque forceTorque;
745 forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
752 std::map<RobotReader::Hand, robot::ToFArray>
755 std::map<RobotReader::Hand, robot::ToFArray> tofs;
758 const armem::wm::CoreSegment& coreSegment = memory
762 coreSegment.forEachEntity(
763 [&tofs](
const wm::Entity& entity)
767 if (not entity.getLatestSnapshot().hasInstance(0))
771 const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
773 const auto exteroception =
774 tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
778 for (
const auto& [handName, dtoFt] : exteroception->tof)
780 ARMARX_DEBUG <<
"Processing ToF element for hand `" << handName <<
"`";
783 fromAron(dtoFt, tof);
785 const auto hand = fromHandName(handName);
786 tofs.emplace(hand, tof);
793 std::optional<robot::RobotDescription>
795 const std::string& name)
const
798 const armem::wm::ProviderSegment& providerSegment = memory
800 .getProviderSegment(name);
806 if (instance ==
nullptr)
808 ARMARX_VERBOSE <<
"No entity snapshots found in provider segment `" << name <<
"`";
815 std::vector<robot::RobotDescription>
818 const armem::wm::CoreSegment& coreSegment =
821 std::vector<robot::RobotDescription> descriptions;
823 coreSegment.forEachInstance(
826 if (
const std::optional<robot::RobotDescription> desc =
829 descriptions.push_back(desc.value());
836 std::vector<robot::RobotDescription>
854 if (not memoryReader)
856 ARMARX_WARNING <<
"Memory reader is null. Did you forget to call "
857 "RobotReader::connect() in onConnectComponent()?";
863 std::scoped_lock l(memoryReaderMutex);
868 if (not qResult.success)
873 return getRobotDescriptions(qResult.memory);